%% 6.581 PS5
%% Qsn4
%% descent.m

function [phat, error]=descent(func, p0)

%% Implements GRADIENT descent.  This method will plot the progress of the
%% alg. at each iteration.
%% 
%% Input Arguments:
%%
%% func: is a pointer to a function that returns the
%%       cost function and the gradient of the cost function.
%% p0:   is the initial guess for the value of P that minimized the cost
%%       function.
%%
%% Output Arguments:
%%
%% phat:  the p that minimizes the cost function
%% 
%% error: the residual cost evaluated at phat 
%%

N = size(p0,1);

if(size(p0,2) ~= 1) 
    error('p0 must be a column vector');
end
    
% Inequality Constraints
% Ax <= b
A = -1*eye(N);
b = zeros(N,1);

% Equality Constraints
% Aeq * x = beq
Aeq = [];
beq = [];

% Bounds on the design variable
% lb <= x <= ub
lb =    0*ones(N,1);
ub = 1e10*ones(N,1);

% Nonlinear Constraints
nonlcon = [];

% Solver Options.

options = optimset(...
    'GradObj','on', ...         % Our function computes the gradient
    'Display', 'iter', ...      % Display stats at each iteration
    'DerivativeCheck','on', ... % Sanity Check the Derivative
    'LargeScale', 'off',...     % Large Scale Alg wont solve our problem
    'OutputFcn', @outfun);      % Plot the results from each step

% Guess
x0 = (rand(N,1)-1/2)*2*10;

% Minimize
[phat, error, flag] = fmincon(func, p0, A, b, Aeq, beq, lb, ub, nonlcon, options);




%%
%% This function graphs the progress of the optimizer.
%%
function stop = outfun(x, optimValues, state)
stop=[];
persistent param 
persistent gradient
persistent objective


switch state
    case 'init'
        param     = [];
        objective = [];
        gradient  = [];
        
    case 'iter'
        param     = [param, x];
        objective = [objective, optimValues.fval];
        gradient  = [gradient, optimValues.gradient];
        
        subplot(3,1,1);semilogy(0:optimValues.iteration, objective, '-');
        xlabel('Iteration');
        ylabel('Cost Function');
        subplot(3,1,2);plot(0:optimValues.iteration, param,     '-');
        xlabel('Iteration');
        ylabel('Parameter Values');

        subplot(3,1,3);plot(0:optimValues.iteration, gradient,  '-');
        xlabel('Iteration');
        ylabel('dCost d p_i');
        drawnow;

    case 'done'
        assignin('base', 'param', param);
        assignin('base', 'obj',  objective);
        assignin('base', 'grad',  gradient);
otherwise
end