% Gradient descent method to find the minimum of a function
% Patrick Roberts SySc 512 (may 2007)
clear all 
%---  Setup 2-dim feasibility set ---
xmax=4; xmin=-xmax; % range of x-component
ymax=4; ymin=-ymax; % range of y-component 
N=50; M=N;          % number of points in the grid
x=linspace(xmin,xmax,N); y=linspace(ymin,ymax,M); % create grid vectors
[X Y]=meshgrid(x,y);                              % create 2-dim mesh

%---  Create 2-dim objective function ---
sigmaA = 1;  sigmaB = 2;    % Parameters for the objective function
G=@(A,B) sigmaA*A.^2 + sigmaB*B.^2; % Define the objective function 
f=G(X,Y); 

%--- Display 3-dim plot --------
figure(1); clf     % new figure (Clear current figure window)
mesh(X,Y,f);       % Draw  3-dim plot of objecting functions
xlabel('x'); ylabel('y'); zlabel('f'); % label plot

%--- Display contour plot with gradient arrows --------
figure(2); clf      % new figure (Clear current figure window)
contour(X,Y,f)      % Draw contour plot
[gradX,gradY] = gradient(f);    % Compute numerical gradient
hold on
quiver(X,Y,gradX,gradY)         % Draw gradient arrows on contour plot
hold off
xlabel('x'); ylabel('y');   % label plot
grid                        % Draw grid lines on contour plot

disp('========= new run ===========')
disp('Initial point:')
epsilon = 0.1;               % termination test parameter
pt = xmin + (xmax-xmin) * rand(1,2)     % Choose a randon start point
prevPt = [100 100];                     % Choose a distant previous point for starters    
while norm(pt-prevPt) > epsilon         % Termination test
    prevPt = pt;                        % Set previous point for next iteration
%    [xID yID] = meshIndex(prevPt, x, y); % Get index of point
    
     [gradf gradfy] = grad(G, prevPt, 0.001);   % Compute gradient at point
     gradf = [gradf gradfy];
     s_norm = norm(gradf);           % Compute norm of gradient vector
     s = -gradf/s_norm;              % Compute search direction vector
    f =@ (t) G(prevPt(1) + s(1)*t, prevPt(2) + s(2)*t); % Define 1-dim search function
    t = 0;                           % initialize 1-dim parameter             
    while f(t) < f(t-0.1)            % Test for 1-dim minimum
        pt = [prevPt(1) + s(1)*t prevPt(2) + s(2)*t]; % nest point in search direction
        hold on
        plot(pt(1), pt(2), '*')     % Draw next point on contour plot
        hold off
        t = t + 0.1;                % Increment 1-dim search parameter
    end
end
disp('Final point:')
pt                        % Write final point to Command Window.