% function T = FMM(F, initialSet)
% 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 = FMM(initialSet, lambda, k1, k2, m, n)

global HUGE
HUGE = 4.2950e+009;

global dX
global dY
global T
dX = zeros(m,n);
dY = zeros(m,n);

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

F = zeros(m, n);
%Fupdated = zeros(m, n);

% 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;
        % initialize normal vector to the front
        normVec = [1 1];
        % add neighbors of (si, sj) to band
        if sj < n
            F(si, sj+1) = updateSpeed(normVec, lambda, k1(si), k2(sj+1));
            T(si, sj+1,1) = F(si, sj+1);
            band = addToBand(band, T, si, sj+1);
        end
        
        if sj > 1
            F(si, sj-1) = updateSpeed(normVec, lambda, k1(si), k2(sj-1));
            T(si, sj-1,1) = F(si, sj-1);
            band = addToBand(band, T, si, sj-1);
        end
        
        if si < m
            F(si+1, sj) = updateSpeed(normVec, lambda, k1(si+1), k2(sj));
            T(si+1, sj,1) = F(si+1, sj);
            band = addToBand(band, T, si+1, sj);
        end
        
        if si > 1
            F(si-1, sj) = updateSpeed(normVec, lambda, k1(si-1), k2(sj));
            T(si-1, sj,1) = F(si-1, sj);
            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
    
    % compute normal to the front at (imin, jmin)
    normVec = normalVec(T, imin, jmin, m, n);
    if norm(normVec) ~= 0
        dX(imin, jmin) = normVec(2)/norm(normVec);
        dY(imin, jmin) = normVec(1)/norm(normVec);
    end
        
    % compute a neighbor's speed value F if neighbor is not accepted -
    % i.e. if neighbor is far away or in band
    % F at a point is based on the normal vector averaged from point's neighbors 
    % Then, 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
            normVec = avgNormals(imin-1, jmin, m, n);
            F(imin-1, jmin) = updateSpeed(normVec, lambda, k1(imin-1), k2(jmin));
            T(imin-1, jmin, 1) = solveQuadratic(T, F, imin-1, jmin, m, n);
            [band, T] = addToBand(band, T, imin-1, jmin);
        else
            if T(imin-1, jmin, 2) ~= HUGE
                % in band
                normVec = avgNormals(imin-1, jmin, m, n);
                F(imin-1, jmin) = updateSpeed(normVec, lambda, k1(imin-1), k2(jmin));
                [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
            normVec = avgNormals(imin+1, jmin, m, n);
            F(imin+1, jmin) = updateSpeed(normVec, lambda, k1(imin+1), k2(jmin));
            T(imin+1, jmin, 1) = solveQuadratic(T, F, imin+1, jmin, m, n);
            [band, T] = addToBand(band, T, imin+1, jmin);
        else
            if T(imin+1, jmin, 2) ~= HUGE
                % in band
                normVec = avgNormals(imin+1, jmin, m, n);
                F(imin+1, jmin) = updateSpeed(normVec, lambda, k1(imin+1), k2(jmin));
                [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
            normVec = avgNormals(imin, jmin-1, m, n);
            F(imin, jmin-1) = updateSpeed(normVec, lambda, k1(imin), k2(jmin-1));
            T(imin, jmin-1, 1) = solveQuadratic(T, F, imin, jmin-1, m, n);
            [band, T] = addToBand(band, T, imin, jmin-1);
        else
            if T(imin, jmin-1, 2) ~= HUGE
                % in band
                normVec = avgNormals(imin, jmin-1, m, n);
                F(imin, jmin-1) = updateSpeed(normVec, lambda, k1(imin), k2(jmin-1));
                [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
            normVec = avgNormals(imin, jmin+1, m, n);
            F(imin, jmin+1) = updateSpeed(normVec, lambda, k1(imin), k2(jmin+1));
            T(imin, jmin+1, 1) = solveQuadratic(T, F, imin, jmin+1, m, n);
            [band, T] = addToBand(band, T, imin, jmin+1);
        else
            if T(imin, jmin+1, 2) ~= HUGE
                % in band
                normVec = avgNormals(imin, jmin+1, m, n);
                F(imin, jmin+1) = updateSpeed(normVec, lambda, k1(imin), k2(jmin+1));
                [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);


