% c = backtrack(T, step, iInit, jInit)
% returns the path of minimal action that starts at (iInit, jInit)
% and proceeds in steps of size step in direction orthogonal to the 
% level curves of T.
function c = backtrack(T, step, iInit, jInit, coordsY, coordsX)

SMALL = 0.000001;

[m n] = size(T);
cntBound = 2*(m + n)/step;

[dTdx dTdy] = gradient(T, coordsY, coordsX);

c = [iInit jInit];
cNext = [0 0];
cnt = 1;
gradVec = [dTdy(m,n) dTdx(m,n)];
curIdxX = m;
curIdxY = n;
while 1,
    if cnt > cntBound
        disp('backtrack break: Too many steps taken');
        break;
    end
    
    % step in direction opposite that of the gradient
    gradNorm = norm(gradVec);
    if gradNorm < SMALL
        disp('backtrack break: Small gradient magnitude');
        break;
    end
    cNext = c(cnt, 1:2) - step*gradVec/gradNorm;
    c = [c; cNext];
    
    % linearly search for indexes of current grid location
    while curIdxX > 1 & cNext(1) < coordsX(curIdxX),
        curIdxX = curIdxX - 1;
    end
    while curIdxY > 1 & cNext(2) < coordsY(curIdxY),
        curIdxY = curIdxY - 1;
    end

    % out of bounds checks 
    % termination criterion
    if (cNext(1) < 1) | (cNext(2) < 1)
        break;
    end
    if curIdxX == m
        curIdxXNext = m;
        cNext(1) = coordsX(m);
        c(end, 1) = coordsX(m);
        spacingX = 1;
    else
        curIdxXNext = curIdxX+1;
        spacingX = coordsX(curIdxXNext) - coordsX(curIdxX);
    end
    if curIdxY == n
        curIdxYNext = n;
        cNext(2) = coordsY(n);
        c(end, 2) = coordsY(n);
        spacingY = 1;
    else
        curIdxYNext = curIdxY+1;
        spacingY = coordsY(curIdxYNext) - coordsY(curIdxY);
    end
%     if cl(1) > coordsX(m)
%         cl(1) = coordsX(m);
%         c(end, 1) = coordsX(m);
%     end
%     if cl(2) > coordsY(n)
%         cl(2) = coordsY(n);
%         c(end, 2) = coordsY(n);
%     end
    % compute the gradient value at new location by bilinear interpolation
    gradVec = [bilinInterp(dTdy(curIdxX, curIdxY), ...
            dTdy(curIdxXNext, curIdxY), ...
            dTdy(curIdxX, curIdxYNext), ...
            dTdy(curIdxXNext, curIdxYNext), ...
            cNext(1) - coordsX(curIdxX), ...
            cNext(2) - coordsY(curIdxY), ...
            spacingX, spacingY) ...            
            bilinInterp(dTdx(curIdxX, curIdxY), ...
            dTdx(curIdxXNext, curIdxY), ...
            dTdx(curIdxX, curIdxYNext), ...
            dTdx(curIdxXNext, curIdxYNext), ...
            cNext(1) - coordsX(curIdxX), ...
            cNext(2) - coordsY(curIdxY), ...
            spacingX, spacingY)];
%     gradVec = [bilinInterp(dTdy(flr(1), flr(2)), ...
%             dTdy(cl(1), flr(2)), ...
%             dTdy(flr(1), cl(2)), ...
%             dTdy(cl(1), cl(2)), rem(1), rem(2)) ...
%             bilinInterp(dTdx(flr(1), flr(2)), ...
%             dTdx(cl(1), flr(2)), ...
%             dTdx(flr(1), cl(2)), ...
%             dTdx(cl(1), cl(2)), rem(1), rem(2))];
    
    % trick - make sure the gradient lies in the first quadrant
    if gradVec(1) < 0
        gradVec(1) = 0;
    end
    if gradVec(2) < 0
        gradVec(2) = 0;
    end
    
    cnt = cnt+1;
end

function Fab = bilinInterp(F00, F10, F01, F11, a, b, aSpacing, bSpacing)

Fab = (bSpacing - b)*(aSpacing - a)*F00 + (bSpacing - b)*a*F10 + b*(aSpacing - a)*F01 + b*a*F10;

