function [ p ] = GradientDescent( Map, p, gui, grad)
% topo2 = imgaussfilt(topo,5);
%GradientDescent(topo2,[167 148],1)

optMeth = -1; %+1 if maximizing, -1 if minimizing
%initAlpha = 2;
%alpha = initAlpha;
alpha = 1;
[Ix,Iy] = gradient(Map);
if gui == 3
    surf(Map);
    hold on;
elseif gui == 2
    contour(Map);
    hold on;
end

for i = 1:100
    oldP = p;
    s = [Ix(p(2),p(1)) Iy(p(2),p(1))];    
    if norm(s) == 0
        disp('optimum');        
        break;
    end
    s = s ./ norm(s);    
    p = p + optMeth * alpha * s;    
    p = matrixEntry(size(Map),p);
    %{
    if optMeth == 1        
        if Map(p(2),p(1))>Map(oldP(2),oldP(1))
            alpha = alpha*1.5;
        else
            alpha = initAlpha;
        end        
    else
        if Map(p(2),p(1))<Map(oldP(2),oldP(1))
            alpha = alpha*1.5;
        else
            alpha = initAlpha;
        end
    end   
    %alpha = alpha*rand()*3;
    %}
    if gui == 3
        res = 50;
        inbetween1 = round(linspace(oldP(1),p(1),res));
        inbetween2 = round(linspace(oldP(2),p(2),res));
        plot3(inbetween1,inbetween2,Map(sub2ind(size(Map),inbetween2,inbetween1)),'LineWidth',2);
        pause(0.5)
    elseif gui == 2
        res = 50;
        inbetween1 = round(linspace(oldP(1),p(1),res));
        inbetween2 = round(linspace(oldP(2),p(2),res));
        plot(inbetween1,inbetween2,'LineWidth',2);
        pause(0.5)
    end
end

hold off;
end

function pos = matrixEntry(sizeOfMatrix,pos)
    pos = round(pos);
    if pos(1)>sizeOfMatrix(2)
        pos(1) = sizeOfMatrix(2);
    end
    if pos(1)<1
        pos(1) = 1;
    end
    if pos(2)>sizeOfMatrix(1)
        pos(2) = sizeOfMatrix(1);
    end
    if pos(2)<1
        pos(2) = 1;
    end    
end

