function msfm2d(FSpeedImage, SourcePoints, usesecond, usecross)
# This function MSFM2D calculates the shortest distance from a list of
# points to all other pixels in an image, using the
# Multistencil Fast Marching Method (MSFM). This method gives more accurate
# distances by using second order derivatives and cross neighbours.
#
# T=msfm2d(F, SourcePoints, UseSecond, UseCross)
#
# inputs,
#   F: The speed image. The speed function must always be larger
#			than zero (min value 1e-8), otherwise some regions will
#			never be reached because the time will go to infinity.
#   SourcePoints : A list of starting points [2 x N] (distance zero)
#   UseSecond : Boolean Set to true if not only first but also second
#                order derivatives are used (default)
#   UseCross : Boolean Set to true if also cross neighbours
#                are used (default)
# outputs,
#   T : Image with distance from SourcePoints to all pixels
#
# Note:
#   Compile the c file "mex msfm2d.c"  for cpu-effective registration
#
# Literature : M. Sabry Hassouna et Al. Multistencils Fast Marching
#   Methods: A Highly Accurate Solution to the Eikonal Equation on
#   Cartesian Domains
#
# Example,
#   SourcePoint = [51; 51];
#   SpeedImage = ones([101 101]);
#   [X Y] = ndgrid(1:101, 1:101);
#   T1 = sqrt((X-SourcePoint(1)).^2 + (Y-SourcePoint(2)).^2);
#
#   # Run fast marching 1th order, 1th order multi stencil
#   # and 2th orde and 2th orde multi stencil
#
#   tic; T1_FMM1 = msfm2d(SpeedImage, SourcePoint, false, false); toc;
#   tic; T1_MSFM1 = msfm2d(SpeedImage, SourcePoint, false, true); toc;
#   tic; T1_FMM2 = msfm2d(SpeedImage, SourcePoint, true, false); toc;
#   tic; T1_MSFM2 = msfm2d(SpeedImage, SourcePoint, true, true); toc;
#
#   # Show results
#   fprintf('\nResults with T1 (Matlab)\n');
#   fprintf('Method   L1        L2        Linf\n');
#   Results = cellfun(@(x)([mean(abs(T1(:)-x(:))) mean((T1(:)-x(:)).^2) max(abs(T1(:)-x(:)))]), {T1_FMM1(:) T1_MSFM1(:) T1_FMM2(:) T1_MSFM2(:)}, 'UniformOutput',false);
#   fprintf('FMM1:   %9.5f %9.5f %9.5f\n', Results{1}(1), Results{1}(2), Results{1}(3));
#   fprintf('MSFM1:  %9.5f %9.5f %9.5f\n', Results{2}(1), Results{2}(2), Results{2}(3));
#   fprintf('FMM2:   %9.5f %9.5f %9.5f\n', Results{3}(1), Results{3}(2), Results{3}(3));
#   fprintf('MSFM2:  %9.5f %9.5f %9.5f\n', Results{4}(1), Results{4}(2), Results{4}(3));
#
# Example multiple starting points,
#   SourcePoint=rand(2,100)*255+1;
#   SpeedImage = ones([256 256]);
#   tic; T1_MSFM2 = msfm2d(SpeedImage, SourcePoint, true, true); toc;
#   figure, imshow(T1_MSFM2,[]); colormap(hot(256));
#
# Function is written by D.Kroon University of Twente (June 2009)

# Distance image, also used to store the index of narrowband pixels
# during marching process
T = zeros(size(F))-1;

# Augmented Fast Marching (For skeletonize)
Ed=nargout>1;

# Euclidian distance image
if(Ed)
	Y = zeros(size(F))
 end

# Pixels which are processed and have a final distance are frozen
Frozen   = zeros(size(F));

# Free memory to store neighbours of the (segmented) region
neg_free = 100000;
neg_pos=0;
if(Ed)
    neg_list = zeros(4,neg_free);
else
    neg_list = zeros(3,neg_free);
end

# (There are 3 pixel classes:
#   - frozen (processed)
#   - narrow band (boundary) (in list to check for the next pixel with smallest distance)
#   - far (not yet used)

# Neighbours
ne =[-1 0;
    1 0;
    0 -1;
    0 1];

SourcePoints=int32(floor(SourcePoints));

# set all starting points to distance zero and frozen
for z=1:size(SourcePoints,2)
    # starting point
    x= SourcePoints(1,z); y=SourcePoints(2,z);
    # Set starting point to frozen and distance to zero
    Frozen(x,y)=1; T(x,y)=0;
end

# Add all neighbours of the starting points to narrow list
for z=1:size(SourcePoints,2)
    # starting point
    x=SourcePoints(1,z);
	y=SourcePoints(2,z);
    for k=1:4,
        # Location of neighbour
        i=x+ne(k,1); j=y+ne(k,2);
        # Check if current neighbour is not yet frozen and inside the
        # picture
        if((i>0)&&(j>0)&&(i<=size(F,1))&&(j<=size(F,2))&&(Frozen(i,j)==0))
            Tt=1/max(F(i,j),eps);
	        Ty=1;
            # Update distance in neigbour list or add to neigbour list
            if(T(i,j)>0)
				if(neg_list(1,T(i,j))>Tt)
					neg_list(1,T(i,j))=Tt;
	            end
				if(Ed)
                    neg_list(4,T(i,j))=min(Ty,neg_list(4,T(i,j)));
                end
            else
	            neg_pos=neg_pos+1;
                # If running out of memory at a new block
                if(neg_pos>neg_free)
			 neg_free = neg_free +100000;
			 neg_list(1,neg_free)=0;
	         end
                if(Ed)
                    neg_list(:,neg_pos)=[Tt;i;j;Ty];
                else
                    neg_list(:,neg_pos)=[Tt;i;j];
                end
                T(i,j)=neg_pos;
            end
        end
    end
end

# Loop through all pixels of the image
for itt=1:numel(F)
    # Get the pixel from narrow list (boundary list) with smallest
    # distance value and set it to current pixel location
    [t,index]=min(neg_list(1,1:neg_pos));
    if(neg_pos==0)
	 break;
    end
    x=neg_list(2,index); y=neg_list(3,index);
    Frozen(x,y)=1;
    T(x,y)=neg_list(1,index);

    if(Ed)
	 Y(x,y)=neg_list(4,index);
    end

    # Remove min value by replacing it with the last value in the array
    if(index<neg_pos)
        neg_list(:,index)=neg_list(:,neg_pos);
        x2=neg_list(2,index); y2=neg_list(3,index);
        T(x2,y2)=index;
    end
    neg_pos =neg_pos-1;

    # Loop through all 4 neighbours of current pixel
    for k=1:4,
        # Location of neighbour
        i=x+ne(k,1); j=y+ne(k,2);

        # Check if current neighbour is not yet frozen and inside the
        # picture
        if((i>0)&&(j>0)&&(i<=size(F,1))&&(j<=size(F,2))&&(Frozen(i,j)==0))

            Tt=CalculateDistance(T,F(i,j),size(F),i,j,usesecond,usecross,Frozen);
            if(Ed)
                Ty=CalculateDistance(Y,1,size(F),i,j,usesecond,usecross,Frozen);
            end

            # Update distance in neigbour list or add to neigbour list
            if(T(i,j)>0)
                neg_list(1,T(i,j))=min(Tt,neg_list(1,T(i,j)));
                if(Ed)
                    neg_list(4,T(i,j))=min(Ty,neg_list(4,T(i,j)));
                end
            else
                neg_pos=neg_pos+1;
                # If running out of memory at a new block
                if(neg_pos>neg_free)
			 neg_free = neg_free +100000;
			 neg_list(1,neg_free)=0;
	        end
                if(Ed)
                    neg_list(:,neg_pos)=[Tt;i;j;Ty];
                else
                    neg_list(:,neg_pos)=[Tt;i;j];
                end
                T(i,j)=neg_pos;
            end
        end
    end
end

function CalculateDistance(T,Fij,sizeF,i,j,usesecond,usecross,Frozen)
# Boundary and frozen check -> current patch
Tpatch=inf(5,5);
for nx=-2:2
    for ny=-2:2
        in=i+nx; jn=j+ny;
        if((in>0)&&(jn>0)&&(in<=sizeF(1))&&(jn<=sizeF(2))&&(Frozen(in,jn)==1))
            Tpatch(nx+3,ny+3)=T(in,jn);
        end
    end
end
end

# The values in order is 0 if no neighbours in that direction
# 1 if 1e order derivatives is used and 2 if second order
# derivatives are used
Order=zeros(1,4);

# Make 1e order derivatives in x and y direction
Tm(1) = min( Tpatch(2,3) , Tpatch(4,3));
if(isfinite(Tm(1)))
 Order(1)=1;
 end
Tm(2) = min( Tpatch(3,2) , Tpatch(3,4));
if(isfinite(Tm(2)))
 Order(2)=1;
end
# Make 1e order derivatives in cross directions
if(usecross)
    Tm(3) = min( Tpatch(2,2) , Tpatch(4,4));
     if(isfinite(Tm(3)))
      Order(3)=1;
      end
    Tm(4) = min( Tpatch(2,4) , Tpatch(4,2));
    	  if(isfinite(Tm(4)))
		 Order(4)=1;
	   end
end

# Make 2e order derivatives
if(usesecond)
    Tm2=zeros(1,4);
    # pixels with a pixeldistance 2 from the center must be
    # lower in value otherwise use other side or first order
    ch1=(Tpatch(1,3)<Tpatch(2,3))&&isfinite(Tpatch(2,3)); ch2=(Tpatch(5,3)<Tpatch(4,3))&&isfinite(Tpatch(4,3));

    if(ch1&&ch2)
	Tm2(1) =min( (4*Tpatch(2,3)-Tpatch(1,3))/3 , (4*Tpatch(4,3)-Tpatch(5,3))/3);
	 Order(1)=2;
    elseif(ch1)
	 Tm2(1) =(4*Tpatch(2,3)-Tpatch(1,3))/3; Order(1)=2;
    elseif(ch2)
	 Tm2(1) =(4*Tpatch(4,3)-Tpatch(5,3))/3; Order(1)=2;
    end

    ch1=(Tpatch(3,1)<Tpatch(3,2))&&isfinite(Tpatch(3,2)); ch2=(Tpatch(3,5)<Tpatch(3,4))&&isfinite(Tpatch(3,4));

    if(ch1&&ch2)
	Tm2(2) =min( (4*Tpatch(3,2)-Tpatch(3,1))/3 , (4*Tpatch(3,4)-Tpatch(3,5))/3);
	Order(2)=2;
    elseif(ch1)
	 Tm2(2)=(4*Tpatch(3,2)-Tpatch(3,1))/3; Order(2)=2;
    elseif(ch2)
	 Tm2(2)=(4*Tpatch(3,4)-Tpatch(3,5))/3; Order(2)=2;
    end

    if(usecross)
        ch1=(Tpatch(1,1)<Tpatch(2,2))&&isfinite(Tpatch(2,2)); ch2=(Tpatch(5,5)<Tpatch(4,4))&&isfinite(Tpatch(4,4));
        if(ch1&&ch2)
		Tm2(3) =min( (4*Tpatch(2,2)-Tpatch(1,1))/3 , (4*Tpatch(4,4)-Tpatch(5,5))/3); Order(3)=2;
        elseif(ch1)
		 Tm2(3)=(4*Tpatch(2,2)-Tpatch(1,1))/3; Order(3)=2;
        elseif(ch2)
		 Tm2(3)=(4*Tpatch(4,4)-Tpatch(5,5))/3; Order(3)=2;
        end

        ch1=(Tpatch(1,5)<Tpatch(2,4))&&isfinite(Tpatch(2,4)); ch2=(Tpatch(5,1)<Tpatch(4,2))&&isfinite(Tpatch(4,2));
        if(ch1&&ch2)
		Tm2(4) =min( (4*Tpatch(2,4)-Tpatch(1,5))/3 , (4*Tpatch(4,2)-Tpatch(5,1))/3); Order(4)=2;
        elseif(ch1)
		 Tm2(4)=(4*Tpatch(2,4)-Tpatch(1,5))/3; Order(4)=2;
        elseif(ch2)
		 Tm2(4)=(4*Tpatch(4,2)-Tpatch(5,1))/3; Order(4)=2;
        end
    end
else
    Tm2=zeros(1,4);
end

# Calculate the distance using x and y direction
Coeff = [0 0 -1/(max(Fij^2,eps))];
for t=1:2
    if Order(t) == 1
            Coeff=Coeff+[1 -2*Tm(t) Tm(t)^2];
    elseif Order(t) ==2
            Coeff=Coeff+[1 -2*Tm2(t) Tm2(t)^2]*(2.2500);
    end
end

Tt=roots(Coeff); Tt=max(Tt);
# Calculate the distance using the cross directions
if(usecross)
    Coeff = Coeff + [0 0 -1/(max(Fij^2,eps))];
    for t=3:4;
    	if Order(t) == 1
                Coeff=Coeff+0.5*[1 -2*Tm(t) Tm(t)^2];
	elseif Order(t) == 2
                Coeff=Coeff+0.5*[1 -2*Tm2(t) Tm2(t)^2]*(2.2500);
        end
    end
    Tt2=roots(Coeff); Tt2=max(Tt2);
    # Select minimum distance value of both stensils
    if(~isempty(Tt2))
	 Tt=min(Tt,Tt2);
    end
end

# Upwind condition check, current distance must be larger
# then direct neighbours used in solution
DirectNeigbInSol=Tm(isfinite(Tm));
if(nnz(DirectNeigbInSol>=Tt)>0) # Will this ever happen?
    Tt=min(DirectNeigbInSol)+(1/(max(Fij,eps)));
end




#function roots(Coeff)
#    a=Coeff(1);
#    b=Coeff(2);
#    c=Coeff(3);
#    d=max((b*b)-4.0*a*c,0);
#    if a != 0
#        z(1)= (-b - sqrt(d)) / (2.0*a);
#        z(2)= (-b + sqrt(d)) / (2.0*a);
#    else
#        z(1)= (2.0*c)/(-b - sqrt(d));
#        z(2)= (2.0*c)/(-b + sqrt(d));
#    end
#end

end
end
