% T = FMM(initialSet, F)
% solves the eikonal equation on a 2D grid:
% norm(grad(T(i, j))) = F(i, j), i = 1..m, j = 1..n
% for T() using the fast marching method
function T = FMMNonSquareGrid(initialSet, F, spacing1, spacing2)

global HUGE
HUGE = 4.2950e+009;

global T

[m, n] = size(F);
band = [];
T = HUGE * ones(m, n, 2);

% initialize T
for i=1:size(initialSet,1)
    si = initialSet(i,1);
    sj = initialSet(i,2);
    if (si>0) & (si<=m) & (sj>0) & (sj<=n)
        % initialize T
        T(si, sj, 1) = 0;
        % add neighbors of (si, sj) to band
        if sj < n
            T(si, sj+1,1) = F(si, sj+1)*spacing2(sj);
            band = addToBand(band, T, si, sj+1);
        end
        
        if sj > 1
            T(si, sj-1,1) = F(si, sj-1)*spacing2(sj-1);
            band = addToBand(band, T, si, sj-1);
        end
        
        if si < m
            T(si+1, sj,1) = F(si+1, sj)*spacing1(si);
            band = addToBand(band, T, si+1, sj);
        end
        
        if si > 1
            T(si-1, sj,1) = F(si-1, sj)*spacing1(si-1);
            band = addToBand(band, T, si-1, sj);
        end
    end
end

% march
while size(band,1) ~= 0, % while band not empty
    % get smallest elt in band (topmost)
    Tmin = band(1, 1);
    imin = band(1, 2);
    jmin = band(1, 3);
    
    % accept it
    T(imin, jmin, 1) = Tmin;
    % mark as accepted
    T(imin, jmin, 2) = HUGE;
    
    % remove from band
    if size(band,1) > 1
        [band, T] = getFromBand(band, T);
    else
        band = [];
    end
    
    % Add neighbors to band if they are far away
    % if they are in band - update
    if imin ~= 1
        if T(imin-1, jmin, 1) == HUGE
            % far away
            T(imin-1, jmin, 1) = solveQuadraticNonSquareGrid(T, F, imin-1, jmin, m, n, spacing1, spacing2);
            [band, T] = addToBand(band, T, imin-1, jmin);
        else
            if T(imin-1, jmin, 2) ~= HUGE
                % in band
                [band, T] = updateNode(band, T, F, T(imin-1, jmin, 2), m, n);
            end
        end
    end
    
    if imin ~= m
        if T(imin+1, jmin, 1) == HUGE
            % far away
            T(imin+1, jmin, 1) = solveQuadraticNonSquareGrid(T, F, imin+1, jmin, m, n, spacing1, spacing2);
            [band, T] = addToBand(band, T, imin+1, jmin);
        else
            if T(imin+1, jmin, 2) ~= HUGE
                % in band
                [band, T] = updateNode(band, T, F, T(imin+1, jmin, 2), m, n);
            end
        end
    end
    
    if jmin ~= 1
        if T(imin, jmin-1, 1) == HUGE
            % far away
            T(imin, jmin-1, 1) = solveQuadraticNonSquareGrid(T, F, imin, jmin-1, m, n, spacing1, spacing2);
            [band, T] = addToBand(band, T, imin, jmin-1);
        else
            if T(imin, jmin-1, 2) ~= HUGE
                % in band
                [band, T] = updateNode(band, T, F, T(imin, jmin-1, 2), m, n);
            end
        end
    end
    
    if jmin ~= n
        if T(imin, jmin+1, 1) == HUGE
            % far away
            T(imin, jmin+1, 1) = solveQuadraticNonSquareGrid(T, F, imin, jmin+1, m, n, spacing1, spacing2);
            [band, T] = addToBand(band, T, imin, jmin+1);
        else
            if T(imin, jmin+1, 2) ~= HUGE
                % in band
                [band, T] = updateNode(band, T, F, T(imin, jmin+1, 2), m, n);
            end
        end
    end
    
end % while size(band,1) ~= 0, % while band not empty

T = T(1:end, 1:end, 1);


