start=[3,7]
start= [8,2]
goal=[8,8]

d=[-1,-1,-1,-1,-1,-1,-1,-1,-1,-1
    -1,-1,inf,inf,-1,inf,inf,inf,inf,-1
    -1,inf,inf,inf,-1,inf,inf,inf,inf,-1
    -1,inf,inf,inf,-1,-1,inf,inf,inf,-1
    -1,inf,-1,inf,inf,-1,-1,inf,-1,-1
    -1,inf,-1,inf,-1,-1,-1,inf,-1,-1
    -1,inf,-1,inf,inf,inf,inf,inf,inf,-1
    -1,inf,-1,inf,inf,inf,inf,inf,inf,-1
    -1,-1,-1,-1,-1,-1,-1,-1,-1,-1];
latexmat(d, '%g')
d(start(1),start(2))= 0;
d;
%make d template to show changes
dvt= d;
for e= 1:size(d,1)
    for f= 1:size(d,2)
        if dvt(e,f)>-1
            dvt(e,f)= NaN;
        end
    end    
end    
dvt;
d;
changed = true;
s2= sqrt(2);
step=-1;
while changed 
    step=step+1
    changed= false;
    dv= dvt;
    for y = size(d,1)-1:-1:2%forward scan
        for x = 2:size(d,2)-1
            if(d(y,x) >-1)
                a=[d(y,x)];
                if(d(y,x-1) >-1)
                    a=[a d(y,x-1)+1];
                end
                if(d(y,x+1) >-1)
                    a=[a d(y,x+1)+s2];
                end
                if(d(y+1,x)>-1)
                    a=[a d(y+1,x)+1];
                end
                if d(y+1,x-1) >-1
                    a=[a d(y+1,x-1)+s2];
                end
                
                mi = min(a);
                if(mi~=d(y,x));
                    changed = true;
                end
                if(d(y,x)~=mi)
                     dv(y,x)=mi;
                end    
                d(y,x) = mi;
               
                
            end
        end
    end
    forward=0
    d;
    dv;
    latexmat(dv, '%g')
    dv=dvt;
    for y = 2:size(d,1)-1%backward scan
        for x = size(d,2)-1:-1:2
            if(d(y,x) >-1)
                a=[d(y,x)];
                if(d(y,x+1) >-1)
                    a=[a d(y,x+1)+1];
                end
                if(d(y-1,x-1) >-1)
                    a=[a d(y-1,x-1)+s2];
                end
                if(d(y-1,x)>-1)
                    a=[a d(y-1,x)+1];
                end
                if d(y-1,x+1) >-1
                    a=[a d(y-1,x+1)+s2];
                end
                mi = min(a);
                if(mi~=d(y,x))
                    changed = true;
                end
                if(d(y,x)~=mi)
                     dv(y,x)=mi;
                end   
                d(y,x) = mi;
                
            end
        end
    end
    backward=0
    step
    d;
    dv;
   latexmat(dv, '%g')
end
%plot gradient
colormap(gray)
imagesc(d)

%get gradient path.
cp= goal;
path =goal;


while  ~isequal(cp,start)
       neighbours=[];
       for x = [cp(1)-1:cp(1)+1]
           for y=[cp(2)-1:cp(2)+1]
               if  ~isequal([x,y],cp) && (d(x,y)>-1) 
                   neighbours = cat(1,neighbours,[x,y]);
               end
           end    
       end
       nv=[];
       for i = 1:size(neighbours,1)
        yy=neighbours(i,:);
            nv = [nv d(yy(1),yy(2))];
       end 
       [m,minindex] = min(nv);
       m;
       cp= neighbours(minindex,:);
       path = cat(1,cp,path);
end    
path
latexmat(path, '%g')