// ----------------------------------------------------------------------------------------
// dt_ret_code runBS(DSt_cube<T> *cube_to_xfrm, dt_options options=0)
//
//	Description: Performs distance transform in 'cube_to_xfrm', using 'boundary_cube' 
//		for determining where pores are.
//
//	Valid options:		DT_BOUNDARY_IS_SOLID	-> Cube boundary will be solid rather than mirror
//						DT_INIT_INPUT			-> Will initialize 'cube_to_xfrm'
// ----------------------------------------------------------------------------------------
template <class T>
dt_ret_code DistanceXfrm<T>::runBS(DSt_cube<T> *cube_to_xfrm, dt_options options/*=0*/)
{	
	// Check that cube dimensions match
	if ( (cube_to_xfrm->wX != bound_cube->wX) ||
		 (cube_to_xfrm->wY != bound_cube->wY) ||
		 (cube_to_xfrm->wZ != bound_cube->wZ) )
	{
		return DT_MISMATCHED_SIZES;
	}
		
	unsigned short	x, y, z, x_prev, y_prev, z_prev;
	char	lz, uz, ly, uy, lx, ux, dx, dy, dz;
	T		min, tmp;

	// Setup progress bar update interval, if progress bar provided
	float	prog_step_size = 0.0f, prog_cumulative = 0.0f;
	if ( progress_bar_ptr )
	{	//There are two main passes over entire cube (plus one more if init cube is requested)
		progress_bar_ptr->SetPos(0);
		progress_bar_ptr->SetRange(0, 100 );
		prog_step_size = 50.0f/bound_cube->wZ;
	}

	// Check what options are set
	if ( options & DT_INIT_INPUT )
	{	// Init cube, all walls marked as infinity, pores as 0
		initXfrmCube(cube_to_xfrm);
	}

	if (options & DT_BOUNDARY_IS_SOLID)
	{	// Init all boundaries that are pore to dt_face values.  Note that we want
		// to go down one slice at a time in case disc cache is used.  Do reverse
		// incase disc cache is used (more efficient for next step)

		// If input is only one slice in any direction, assume it is 2D and only
		// a rectangle rather than cube will be used for the solid boundary
		if ( (bound_cube->dimensions.z == 1) ||
			 (bound_cube->dimensions.y == 1) ||
			 (bound_cube->dimensions.x == 1) )
		{
			unsigned int	cur=0, end=3;
			unsigned short	range_table[12][3][2] =
			{	{	{0,1},								{0,1},								{0,bound_cube->wX}					},
				{	{0,1},								{bound_cube->wY-1,bound_cube->wY},	{0,bound_cube->wX}					},
				{	{0,1},								{0,bound_cube->wY},					{0,1}								},
				{	{0,1},								{0,bound_cube->wY},					{bound_cube->wX-1,bound_cube->wX}	},

				{	{0,1},								{0,1},								{0,bound_cube->wX}					},
				{	{bound_cube->wZ-1,bound_cube->wZ},	{0,1},								{0,bound_cube->wX}					},
				{	{0,bound_cube->wZ},					{0,1},								{0,1}								},
				{	{0,bound_cube->wZ},					{0,1},								{bound_cube->wX-1,bound_cube->wX}	},

				{	{0,1},								{0,bound_cube->wY},					{0,1}								},
				{	{bound_cube->wZ-1,bound_cube->wZ},	{0,bound_cube->wY},					{0,1}								},
				{	{0,bound_cube->wZ},					{0,1},								{0,1}								},
				{	{0,bound_cube->wZ},					{bound_cube->wY-1,bound_cube->wY},	{0,1}								}	};

			// If z == 1, we use initialized values for cur/end
			if ( bound_cube->dimensions.y == 1 )
			{
				cur = 4;		end = 7;
			}
			if ( bound_cube->dimensions.x == 1 )
			{
				cur = 8;		end = 11;
			}

			while ( cur <= end )
			{
				for ( z = range_table[cur][0][0]; z < range_table[cur][0][1]; z++ )
				{
					for ( y = range_table[cur][1][0]; y < range_table[cur][1][1]; y++ )
					{
						for ( x = range_table[cur][2][0]; x < range_table[cur][2][1]; x++ )
						{
							if ( bound_cube->get_spot(z, y, x) == pore_value )
							{	// If pore, then mark with face value
								cube_to_xfrm->datac(z, y, x, dt_face);
							}
						}
					}
				}

				++ cur;
			}
		}
		else
		{
			z_prev = cube_to_xfrm->wZ;
			for ( z = cube_to_xfrm->wZ-1; z < z_prev; z-- )
			{
				z_prev = z;
				// For first and last slice, entire slice needs to be examined
				if ( (z==0) || (z==(cube_to_xfrm->wZ-1)) )
				{
					for ( y = 0; y < cube_to_xfrm->wY; y++ )
						for ( x = 0; x < cube_to_xfrm->wX; x++ )
						{
							if ( bound_cube->get_spot(z, y, x) == pore_value )
							{	// If pore, then mark with face value
								cube_to_xfrm->datac(z, y, x, dt_face);
							}
						}
				}
				else
				{
					for ( y = 0; y < cube_to_xfrm->wY; y++ )
					{
						// For first and last row, we want to look at entire row
						if ( (y==0) || (y==(cube_to_xfrm->wY-1)) )
						{
							for ( x = 0; x < cube_to_xfrm->wX; x++ )
							{
								if ( bound_cube->get_spot(z, y, x) == pore_value )
								{	// If pore, then mark with face value
									cube_to_xfrm->datac(z, y, x, dt_face);
								}
							}
						}
						else
						{
							// For each row, just initialize first and last 'x'
							// If pore, then mark with face value
							if ( bound_cube->get_spot(z, y, 0) == pore_value )
							{
								cube_to_xfrm->datac(z, y, 0, dt_face);
							}
							if ( bound_cube->get_spot(z, y, cube_to_xfrm->wX-1) == pore_value )
							{
								cube_to_xfrm->datac(z, y, cube_to_xfrm->wX-1, dt_face);
							}
						}
					}
				}
			}
		}
	}

	// Pass 1, start top, upper, left corner
	for (z = 0; z < cube_to_xfrm->wZ; z++)
	{		
		// Set up limits (if on boundary)
		lz = (z > 0) ? (-1):(0);
		uz = (z < (cube_to_xfrm->wZ - 1) ) ? (1):(0);

		for (y = 0; y < cube_to_xfrm->wY; y++)
		{	
			ly = (y > 0) ? (-1):(0);
			uy = (y < (cube_to_xfrm->wY - 1) ) ? (1):(0);

			for (x = 0; x < cube_to_xfrm->wX; x++)
			{				
				lx = (x > 0) ? (-1):(0);
				ux = (x < (cube_to_xfrm->wX - 1) ) ? (1):(0);

				// Find minimum distance neighbor
				if (cube_to_xfrm->datac(z, y, x) > 0)
				{
					min = cube_to_xfrm->datac(z, y, x);
					for (dz = lz; dz <= uz; dz++)
						for (dy = ly; dy <= uy; dy++)
							for (dx = lx; dx <= ux; dx++)
							{
								if (cube_to_xfrm->datac(z+dz, y+dy, x+dx) != dt_infinity)
								{
									if (dz && dy && dx)
									{	// We are currently looking at a CORNER	neighbor
										tmp = dt_corner + cube_to_xfrm->datac(z+dz, y+dy, x+dx);
										if (tmp < min)
											min = tmp;
									}
									else if ( (dz&&dy) || (dz&&dx) || (dy&&dx) )
									{	// We are currently looking at an EDGE neighbor
										tmp = dt_edge + cube_to_xfrm->datac(z+dz, y+dy, x+dx);
										if (tmp < min)
											min = tmp;
									}
									else
									{	// We are currently looking at a FACE neighbor
										tmp = dt_face + cube_to_xfrm->datac(z+dz, y+dy, x+dx);
										if (tmp < min)
											min = tmp;
									}
								}
							}

					// Mark spot with minimum that was found
					cube_to_xfrm->datac(z, y, x, min);
				}
			}
		}

		// Check if progress should be updated
		if ( progress_bar_ptr )
		{
			prog_cumulative = prog_cumulative + prog_step_size;
			progress_bar_ptr->SetPos( (int)prog_cumulative );
		}
	}

	// Pass 2, start bottow, lower, right corner
	// WARNING: takes advantage of data underflow to stop
	z_prev = cube_to_xfrm->wZ;
	for (z = cube_to_xfrm->wZ-1; z < z_prev; z--)
	{
		z_prev = z;
		y_prev = cube_to_xfrm->wY;

		// Set up limits
		lz = (z > 0) ? (-1):(0);
		uz = (z < (cube_to_xfrm->wZ - 1) ) ? (1):(0);

		for (y = cube_to_xfrm->wY-1; y < y_prev; y--)
		{			
			y_prev = y;
			x_prev = cube_to_xfrm->wX;

			ly = (y > 0) ? (-1):(0);
			uy = (y < (cube_to_xfrm->wY - 1) ) ? (1):(0);

			for (x = cube_to_xfrm->wX-1; x < x_prev; x--)
			{
				x_prev = x;

				lx = (x > 0) ? (-1):(0);
				ux = (x < (cube_to_xfrm->wX - 1) ) ? (1):(0);

				// Find minimum distance neighbor
				if (cube_to_xfrm->datac(z, y, x) > 0)
				{
					min = cube_to_xfrm->datac(z, y, x);
					for (dz = lz; dz <= uz; dz++)
						for (dy = ly; dy <= uy; dy++)
							for (dx = lx; dx <= ux; dx++)
							{							
								if ( dz && dy && dx )
								{	// We are currently looking at a CORNER	neighbor
									tmp = dt_corner + cube_to_xfrm->datac(z+dz, y+dy, x+dx);
									if (tmp < min)
										min = tmp;
								}
								else if ( (dz&&dy) || (dz&&dx) || (dy&&dx) )
								{	// We are currently looking at an EDGE neighbor
									tmp = dt_edge + cube_to_xfrm->datac(z+dz, y+dy, x+dx);
									if (tmp < min)
										min = tmp;
								}
								else 
								{	// We are currently looking at an FACE neighbor
									tmp = dt_face + cube_to_xfrm->datac(z+dz, y+dy, x+dx);
									if (tmp < min)
										min = tmp;
								}
							}

					// Mark spot with minimum that was found
					cube_to_xfrm->datac(z, y, x, min);
				}
			}
		}

		// Check if progress should be updated
		if ( progress_bar_ptr )
		{
			prog_cumulative = prog_cumulative + prog_step_size;
			progress_bar_ptr->SetPos( (int)prog_cumulative );
		}
	}

	return	DT_NO_ERROR;
};


// ----------------------------------------------------------------------------------------
// dt_ret_code runSS(DSt_cube<T> *cube_to_xfrm, dt_options options=0)
//
//	Description: Performs a single start distance transform in 'cube_to_xfrm', using 'boundary_cube' 
//		for determining where pores are.  Uses 'dt_infinity'.  Start coordinate will be marked '1'.
//
//	Valid options:		DT_FACE_ONLY		-> Force a face only Xfrm
//						DT_INIT_INPUT		-> Will initialize 'cube_to_xfrm'
//						DT_STOP_AT_HIT		-> SS Xfrm will STOP once 'dt_end_value' is encountered
// ----------------------------------------------------------------------------------------
template <class T>
dt_ret_code DistanceXfrm<T>::runSS(DSt_cube<T> *cube_to_xfrm, Vect3usi &start_pt, dt_options options/*=0*/)
{
	// Check that cube dimensions match.
	if ( (cube_to_xfrm->wX != bound_cube->wX) ||
		 (cube_to_xfrm->wY != bound_cube->wY) ||
		 (cube_to_xfrm->wZ != bound_cube->wZ) )
	{
		return DT_MISMATCHED_SIZES;
	}

	// Go through options
	if (options & DT_INIT_INPUT)
	{	// Initialize transform cube.  Fibers will be '0', Pores will be 'dt_infinity'
		initXfrmCube(cube_to_xfrm);
	}

	// If DT_FACE_ONLY specified, we can do the fast SS transform
	if (options & DT_FACE_ONLY)
	{
		return	runFastSS( cube_to_xfrm, start_pt, options );
	}
	// Else run the much slower but general SS transform
	else
	{
		return	runFullSS( cube_to_xfrm, start_pt, options );
	}
};


// ----------------------------------------------------------------------------------------
// dt_ret_code eraseSS( DSt_cube<T> *cube_to_erase, Vect3usi &start_pt, T max_erase_value )
//
//	Description: This will revert any interconnected SS field to all 'dt_infinity' values.
//
//	Options:	start_pt		->	Will erase the field that is around this point.
//				erase_value		->	Will not erase any coordinates above this value.  If it
//									is 'dt_infinity', then entire cube will be initialized.
//									Also does not erase any '0' values.
//				DT_INIT_INPUT	->	Will initialize entire cube.
//
// ----------------------------------------------------------------------------------------
template <class T>
dt_ret_code	DistanceXfrm<T>::eraseSS( DSt_cube<T> *cube_to_erase, Vect3usi &start_pt, T max_erase_value, dt_options options/*=0*/ )
{
	// Check that cube dimensions match.
	if ( (cube_to_erase->wX != bound_cube->wX) ||
		 (cube_to_erase->wY != bound_cube->wY) ||
		 (cube_to_erase->wZ != bound_cube->wZ) )
	{
		return DT_MISMATCHED_SIZES;
	}

	// Check to see if entire cube should be initialized
	if ( (max_erase_value == dt_infinity) || (options & DT_INIT_INPUT) )
	{	// Initialize transform cube.  Fibers will be '0', Pores will be 'dt_infinity'
		initXfrmCube(cube_to_erase);
		return	DT_NO_ERROR;
	}

	char			dz, dy, dx, xl, xg, yl, yg, zl, zg;
	unsigned short	x, y, z;
	Vect3usi		cur_neigh;

	// Allocate queue list if it hasn't been allocated yet.
	if ( queue_cur == NULL )
	{
		queue_cur = new LinkedList<Vect3usi>( 20000 );
	}

	// Insert starting point into queue_cur
	queue_cur->insertAtStart( start_pt );

	// Delete the start point
	if ( (cube_to_erase->datac(start_pt) <= max_erase_value) && (cube_to_erase->datac(start_pt) > 0) )
	{
		cube_to_erase->datac(start_pt, dt_infinity);
	}

	// Now we will iterate through queue_cur until it is empty
	while ( queue_cur->getSize() > 0 )
	{
		// For convenience transfer current coordinates to zyx
		(queue_cur->getHeadPtr())->item.to_zyx(z, y, x);

		// No longer need this coordinate in queue
		queue_cur->removeHeadNode( );

		if ( options & DT_FACE_ONLY )
		{
			cur_neigh.from_zyx(z, y, x);
			// For all 6 face neighbors
			for ( xl = 0; xl < 6; xl++ )
			{
				if ( !get_zyx_neigh_6dir(xl, cur_neigh, bound_cube->dimensions) )
				{	// Skip this iteration if out of bounds.
					continue;
				}

				// Make sure spot is a pore and is valid for erasing
				if ( (bound_cube->get_spot(cur_neigh) == pore_value) && 
					 (cube_to_erase->datac(cur_neigh) <= max_erase_value) &&
					 (cube_to_erase->datac(cur_neigh) > 0) )
				{
					// Erase it and add to end of queue_cur
					cube_to_erase->datac(cur_neigh, dt_infinity);
					queue_cur->insertAtEnd( cur_neigh );
				}
			}
		}
		else
		{
			// Boundary check
			xl=-1; xg=1; yl=-1; yg=1; zl=-1; zg=1;
			if (x < 1)	xl = 0;
			if (y < 1)	yl = 0;
			if (z < 1)	zl = 0;
			if (x > bound_cube->wX-2)	xg = 0;
			if (y > bound_cube->wY-2)	yg = 0;
			if (z > bound_cube->wZ-2)	zg = 0;

			// Check if any neighbors have not been erased yet
			for (dz = zl; dz <= zg; dz++)
			{
				for (dy = yl; dy <= yg; dy++)
					for (dx = xl; dx <= xg; dx++)
					{
						// Make sure spot is a pore and is valid for erasing
						cur_neigh.from_zyx(z+dz, y+dy, x+dx);
						if ( (bound_cube->get_spot(cur_neigh) == pore_value) &&
							 (cube_to_erase->datac(cur_neigh) <= max_erase_value) &&
							 (cube_to_erase->datac(cur_neigh) > 0) )
						{
							// Erase it and add to end of queue_cur
							cube_to_erase->datac( cur_neigh, dt_infinity );
							queue_cur->insertAtEnd( cur_neigh );
						}
					}
			}
		}
	}

	return	DT_NO_ERROR;
};

// ----------------------------------------------------------------------------------------
//
//		#####	#####	###	 ##     ##	    #	#########	#######
//		#    #	#    #	 #	  #     #	   ###		#		#
//		#    #	#    #	 #	  ##   ##	  ## ##		#		#
//		#####	#####	 #	   #   #	  #   #		#		#####
//		#		# #		 #	   ## ##	 #######	#		#
//		#		#  #	 #	    ###		 #     #	#		#
//		#		#   #	###		 #		##     ##	#		#######
//
// ----------------------------------------------------------------------------------------


// ----------------------------------------------------------------------------------------
// dt_ret_code initXfrmCube( DSt_cube<T> *cube_to_xfrm )
//
//	Description: Initializes a cube for transform.  Pore spots will be 'dt_infinity',
//		fiber spots will be '0'.
// ----------------------------------------------------------------------------------------
template <class T>
dt_ret_code DistanceXfrm<T>::initXfrmCube( DSt_cube<T> *cube_to_xfrm )
{
	unsigned short	z, y, x;

	// Initialize transform cube.  Fibers will be '0', Pores will be 'dt_infinity'
	for (z = 0; z < bound_cube->wZ; z++)
	{
		for (y = 0; y < bound_cube->wY; y++)
			for (x = 0; x < bound_cube->wX; x++)
			{
				if ( bound_cube->get_spot(z, y, x) == pore_value )
					cube_to_xfrm->datac(z, y, x, dt_infinity);
				else
					cube_to_xfrm->datac(z, y, x, 0);
			}
	}
	
	return	DT_NO_ERROR;
};


// ----------------------------------------------------------------------------------------
// dt_ret_code runFastSS( DSt_cube<T> *cube_to_xfrm )
//
//	Description: This is a fast version of the 1-2-3 FACE only (so really just a '1')
//		single seed (SS) transform.  It is private, and gets called by runSS() if correct
//		criteria are met.
//
//	Options:	DT_STOP_AT_HIT		->	Will stop as soon as a coordinate with 'dt_end_value'
//										is encountered.  Also will treat anything above 'dt_end_value'
//										as a wall.
//
//	Note: 'cube_to_xfrm' is assumed to be initialized and in correct state.
// ----------------------------------------------------------------------------------------
template <class T>
dt_ret_code	DistanceXfrm<T>::runFastSS( DSt_cube<T> *cube_to_xfrm, Vect3usi &start_pt, dt_options options/*=0*/ )
{
	LinkedList<Vect3usi>		*queue_tmp;

	// We need two queues.  The first one (queue_cur) will contain all coordinates that need
	// to be marked with the current distance value.  The second one (queue_nxt) will contain
	// all coordinates that were neighboring the current one, and will be processed once the 
	// current queue is empty.  Allocate queue lists if they haven't been yet.
	if ( queue_cur == NULL )
		queue_cur = new LinkedList<Vect3usi>( 20000 );
	if ( queue_nxt == NULL )
		queue_nxt = new LinkedList<Vect3usi>( 20000 );

	Vect3usi	coord_cur;
	char		i;
	T			cur_p_face;

	// Initialize starting point and queue
	cube_to_xfrm->datac(start_pt, 1);
	
	queue_cur->insertAtStart( start_pt );

	// Now we will iterate through until BOTH queues are empty
	while ( queue_cur->getSize() > 0 )
	{

	#if DT_KEEP_TRACK_OF_STATS
		// Max queue size tracking
		if ( queue_cur->getSize() > ss_fast_max_queue_size )
			ss_fast_max_queue_size = queue_cur->getSize();
	#endif

		coord_cur = (queue_cur->getHeadPtr())->item;
		cur_p_face = dt_face + cube_to_xfrm->datac( coord_cur );

		// If 'cur_p_face' is less than original coord, or above infinity,
		// then an overflow has occured
		if ( (cur_p_face >= dt_infinity) || (cur_p_face < cube_to_xfrm->datac( coord_cur )) )
		{
			return	DT_OVERFLOW_LIKELY;
		}

		// Update furthest point coordinate
		start_pt = coord_cur;

		// Process each coordinate until current queue is empty
		while ( queue_cur->getSize() > 0 )
		{
			// Remove head item from queue
			coord_cur = (queue_cur->getHeadPtr())->item;
			queue_cur->removeHeadNode( );

			// For all 6 face neighbors
			for ( i = 0; i < 6; i++ )
			{
				if ( !get_zyx_neigh_6dir(i, coord_cur, cube_to_xfrm->dimensions) )
				{	// Skip this iteration if out of bounds.
					continue;
				}

				// Check if DT_STOP_AT_HIT options is specified
				if ( options & DT_STOP_AT_HIT )
				{
					// Check if this is an end coordinate
					if ( cube_to_xfrm->datac( coord_cur ) == dt_end_value )
					{	// Clean up and return indicated we had an early stop
						start_pt = coord_cur;
						queue_cur->removeAllNodes( );
						queue_nxt->removeAllNodes( );
						return	DT_STOPPED_AT_HIT;
					}
					// If coordinates value is higher than 'dt_end_value', skip adding
					if ( (cube_to_xfrm->datac(coord_cur) > dt_end_value) &&
						 (cube_to_xfrm->datac(coord_cur) != dt_infinity) )
					{
						continue;
					}
				}

				// Before adding to next queue, make sure the following is
				// true for neighbor coordinate:
				//	1) Current coordinate + distance to neighbor is closer than neighbors current distance.
				//		Note: This can only happen if the spot hasn't been traveled on yet.
				//	2) Is a pore.
				if ( (bound_cube->get_spot(coord_cur) == pore_value) &&
					 (cube_to_xfrm->datac(coord_cur) > cur_p_face) )
				{
					// Update coordinates value and insert at end of next queue.
					cube_to_xfrm->datac( coord_cur, cur_p_face );
					queue_nxt->insertAtStart( coord_cur );
				}
			}
		}

		// Go to next list, note that if nothing was added, we will be done because both are empty
		queue_tmp = queue_cur;
		queue_cur = queue_nxt;
		queue_nxt = queue_tmp;
	}

	return	DT_NO_ERROR;
};


// ----------------------------------------------------------------------------------------
// dt_ret_code runFullSS( DSt_cube<T> *cube_to_xfrm )
//
//	Description: This is a general version of the Single Seed (SS) transform.  It will use
//		the current FEC values of any size in all directions.  It is private, and gets 
//		called by runSS() if correct criteria are met.
//
//	Options:	DT_STOP_AT_HIT		->	Will stop as soon as a coordinate with 'dt_end_value'
//										is encountered.  Also will treat anything above 'dt_end_value'
//										as a wall.
//
//	Note: 'cube_to_xfrm' is assumed to be initialized and in correct state.
// ----------------------------------------------------------------------------------------
template <class T>
dt_ret_code	DistanceXfrm<T>::runFullSS( DSt_cube<T> *cube_to_xfrm, Vect3usi &start_pt, dt_options options/*=0*/)
{
	LinkedListNode<GroupedListNode<T,Vect3usi>>	*group_ptr;
	LinkedList<Vect3usi>		*current_queue;
	LinkedListNode<Vect3usi>	*node_ptr;

	Vect3usi		cur_coord;
	T				current_distance, distance_to_add;
	char			dz, dy, dx, xl, xg, yl, yg, zl, zg;
	unsigned short	x, y, z;
	
	// Allocate group of queues if it hasn't been yet, note that
	// we will never need more than (dt_corner + 1), because min SS in queue
	// will be 1, and max will be dt_corner.
	if ( list_of_queues == NULL )
	{
		list_of_queues = new GroupedLists<T,Vect3usi>( dt_corner+1 );
	}
	
	// Initialize starting point and create first group with a distance of '0'
	cube_to_xfrm->datac( start_pt, 1 );
	
	group_ptr = list_of_queues->getGroup( 1 );
	node_ptr = group_ptr->item.list_ptr->getNewNode();
	node_ptr->item = start_pt;
	list_of_queues->insertIntoGroup( group_ptr, node_ptr );
	
	// Iterate through queues until there are no more
	while ( list_of_queues->getNumberOfGroups() > 0 )
	{
		// Grab current queue (head) and current distance value
		current_queue = (list_of_queues->getHeadGroup())->item.list_ptr;
		current_distance = (list_of_queues->getHeadGroup())->item.group_id;

		// If current distance plus corner (should be largest) is greater than infinity
		// or less than original value, an overflow has occured
		if ( ((current_distance+dt_corner) >= dt_infinity) || ((current_distance+dt_corner) < current_distance) )
		{
			return	DT_OVERFLOW_LIKELY;
		}

	#if DT_KEEP_TRACK_OF_STATS
		// Max queue size tracking
		if ( current_queue->getSize() > ss_full_max_queue_size )
			ss_full_max_queue_size = current_queue->getSize();
	#endif

		// Now go through all of the coordinates in current queue...
		while ( current_queue->getSize() > 0 )
		{
			// If distance value at current coordinate is LESS than current groups
			// distance, this is a duplicate entry and we can skip it.
			if ( current_distance > cube_to_xfrm->datac((current_queue->getHeadPtr())->item) )
			{
				current_queue->removeNode( current_queue->getHeadPtr() );
			#if DT_KEEP_TRACK_OF_STATS
				++ss_full_double_traveled;
			#endif
				continue;
			}

			// Remove current coordinate from queue
			cur_coord = ((current_queue->getHeadPtr())->item);			
			current_queue->removeHeadNode( );

			// Update furthest coordinate to point
			start_pt = cur_coord;
			
			// Do a boundary check
			xl=-1; xg=1; yl=-1; yg=1; zl=-1; zg=1;
			if (cur_coord.x < 1)	xl = 0;
			if (cur_coord.y < 1)	yl = 0;
			if (cur_coord.z < 1)	zl = 0;
			if (cur_coord.x > bound_cube->wX-2)	xg = 0;
			if (cur_coord.y > bound_cube->wY-2)	yg = 0;
			if (cur_coord.z > bound_cube->wZ-2)	zg = 0;

			// For all pore neighbors of current spot...
			for (dz = zl; dz <= zg; dz++)
			{
				for (dy = yl; dy <= yg; dy++)
					for (dx = xl; dx <= xg; dx++)
					{
						// Calculate next neighbors position
						z = cur_coord.z + dz;
						y = cur_coord.y + dy;
						x = cur_coord.x + dx;

						if ( bound_cube->get_spot(z, y, x) == pore_value )
						{
							// Check if we have reached the goal if early stop is requested
							if ( options & DT_STOP_AT_HIT ) 
							{
								if ( cube_to_xfrm->datac(z, y, x) == dt_end_value )
								{
									start_pt.from_zyx(z, y, x);
									list_of_queues->removeAllGroups();
									return	DT_STOPPED_AT_HIT;
								}
								// If coordinates value is higher than 'dt_end_value', skip adding
								if ( (cube_to_xfrm->datac(z, y, x) > dt_end_value) &&
									 (cube_to_xfrm->datac(z, y, x) != dt_infinity) )
								{
									continue;
								}
							}

							distance_to_add = 0;
							// Figure out neighbors orientation, and if cumulative distance is smaller
							// than what is currently in the neighbors spot
							// ----Corner neighbor---- //
							if ( dz && dy && dx )
							{
								if ( (current_distance + dt_corner) < cube_to_xfrm->datac(z, y, x) )
								{
									distance_to_add = dt_corner;
								}
							}
							// ----Face neighbor---- //
							else if ((dz&&(!dy)&&(!dx)) || ((!dz)&&dy&&(!dx)) || ((!dz)&&(!dy)&&dx))
							{
								if ( (current_distance + dt_face) < cube_to_xfrm->datac(z, y, x) )
								{
									distance_to_add = dt_face;
								}
							}
							// ----Edge neighbor---- //
							else
							{
								if ( (current_distance + dt_edge) < cube_to_xfrm->datac(z, y, x) )
								{
									distance_to_add = dt_edge;
								}
							}

							// Now update current neighbors value, and insert into Q.  Note that this can
							// result in multiple insertions of same coordinate if previous value at this spot was not infinity.
							// But it is quicker to just ignore the duplicate later on than to find it and
							// delete it here.
							if ( distance_to_add != 0 )
							{
								cube_to_xfrm->datac(z, y, x, current_distance + distance_to_add);

								group_ptr = list_of_queues->getGroup( current_distance + distance_to_add );
								node_ptr = group_ptr->item.list_ptr->getNewNode();
								node_ptr->item.from_zyx(z, y, x);
								list_of_queues->insertIntoGroup( group_ptr, node_ptr );
							}
						}
					}
			} // END of for(dz,dy,dx)
		}

		// Delete group whos queue was just emptied
		list_of_queues->removeGroup( list_of_queues->getHeadGroup() );
	}

	return	DT_NO_ERROR;
};