%% 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