%% Demonstrate trust region methods

%% Setup data for the Rosenbrock function 
p = problem2d('rosen');
region = [-1.2 -0.8 0.2 1];
x0 = [-1;0.5];

%% Setup data for Trefethen's wild function
% You should only execute code for one of these functions.
p = problem2d('chebfun_opt_manymin');
region = p.region;
x0 = [0; 0.6];

%% Show the contour plot
clf;
ezcontour(p.fonly,region')

%% Show the trust region solutions for various values of Delta
hold on;
deltas = 0.01:0.01:1;
[f0,g,H] = p.f(x0);
ps = zeros(numel(deltas),2);
for i=1:numel(deltas)
    curp = solve_trust_region_subproblem(H,g,deltas(i));
    ps(i,:) = curp';
end
plot(x0(1) + ps(:,1), x0(2) + ps(:,2), 'r.-');
plot(x0(1), x0(2), 'b*','MarkerSize',12);
newton = H\g;
plot([x0(1), x0(1) - g(1)], [x0(2), x0(2) - g(2)], 'b-','LineWidth',3);
plot([x0(1), x0(1) - newton(1)], [x0(2), x0(2) - newton(2)], 'b-','LineWidth',3);

%% Show the contours of the trust region function
trmodel = @(x,y) f0 + ([x(:)' - x0(1);y(:)' - x0(2)])'*g + 1/2*diag(([x(:)' - x0(1);y(:)' - x0(2)])'*H*([x(:)' - x0(1);y(:)' - x0(2)]));
h = ezcontour(trmodel, region');
set(h,'LineWidth',2);

%% Show the Cauchy point's as Delta varies
ps = zeros(numel(deltas),2);
for i=1:numel(deltas)
    delta = deltas(i);
    curpc = -g/norm(g)*delta*min(norm(g)^3/(delta*g'*H*g),delta);
    pc(i,:) = curpc';
end
plot(x0(1) + pc(:,1), x0(2) + pc(:,2), 'y*');

%% Show the dogleg path
pd = zeros(numel(deltas),2);
pfull = -H\g;            % compute the directions we need for the dogleg
pu = -(g'*g/(g'*H*g))*g;
pdiff = pfull - pu;
for i=1:numel(deltas)
    delta = deltas(i);
    % check if the result is in one of the easy cases
    if norm(pfull) < delta, curpd = pfull; 
    elseif norm(pu) > delta, curpd = pu/norm(pu)*delta;
    else
        % find the intersection with the dogleg path by solving the
        % quadratic 
        %   norm(pu - (tau-1)*pdiff)^2 = delta^2 
        %     => pdiff'*pdiff*(tau-1)^2 + 2*(tau-1)*pdiff'*pu +
        %           pu'*pu - delta^2 = 0
        % for (tau-1) 
        tmin1=roots([pdiff'*pdiff,2*pdiff'*pu,pu'*pu - delta*delta]);
        tmin1 = tmin1(tmin1 >= 0 & tmin1 <= 1);
        assert(numel(tmin1) == 1);
        curpd = pu + (tmin1)*pdiff;
    end
    pd(i,:) = curpd';
end
plot(x0(1) + pd(:,1), x0(2) + pd(:,2), 'g*');

%%
clf;
p = problem2d('chebfun_opt_manymin');
ezcontour(p.fonly,p.region');
%%
hold on;
deltas = 0.01:0.01:1;
x0 = [0; 0.6];
[f0,g,H] = p.f(x0);
ps = zeros(numel(deltas),2);
for i=1:numel(deltas)
    curp = solve_trust_region_subproblem(H,g,deltas(i));
    ps(i,:) = curp';
end
plot(x0(1) + ps(:,1), x0(2) + ps(:,2), 'r.-');
plot(x0(1), x0(2), 'b*','MarkerSize',12);
newton = H\g;
plot([x0(1), x0(1) - g(1)], [x0(2), x0(2) - g(2)], 'b-');
plot([x0(1), x0(1) - newton(1)], [x0(2), x0(2) - newton(2)], 'b-');
