function trustdemo
% Demonstrate the trust-region method. 

clf

% Set up the function, the gradient and the Hessian. 
f = @(x) x(1)^2-5*x(1)*x(2)+x(2)^4-25*x(1)-8*x(2);
g = @(x) [2*x(1)-5*x(2)-25;-5*x(1)+4*x(2)^3-8];
H = @(x) [2, -5; -5, 12*x(2)^2];

% Contour plot of the f(x).
Np = 201;
alim = [-10,40,-3,6];
ax = linspace(alim(1),alim(2),Np);
ay = linspace(alim(3),alim(4),Np);
az = zeros(101,101);
for i=1:Np
    for j=1:Np
	x = [ax(i),ay(j)];
	az(i,j) = f(x);
    end
end
contour(ax,ay,az',min(min(az))+10.^(-4:0.5:8),'k');
hold on
axis('square')

% Trust region metod: Choose between
%   - trust ("exact" solution of the TR problem)
%   - cauchypoint 
%   - dogleg

onestep = @dogleg;
Delta = 1;
DeltaMax = 10; 
MaxIter = 100;

% Initial values. 
x = [15;2.6];

% Start of the iterations. 
for n=1:MaxIter;
   gt = g(x);
   if norm(gt)<1.e-8 
       break;
   end
   Ht = H(x); 
   [p,mp] = onestep(gt,Ht,Delta); 
   rho = (f(x)-f(x+p))/(-mp);
   if rho < 0.25
       DeltaNew = 0.5*Delta;
   elseif rho > 0.75
       DeltaNew = min(2*Delta,DeltaMax);
   end
   
   % For the plot
   xtmp = x+p;
   plot([x(1),xtmp(1)],[x(2),xtmp(2)],'x-r')
   
   if rho > 0.01 | norm(g(x+p))<1.e-8
      x = x+p;
   end
   fprintf('n = %3d,  x = [%8.5f, %8.5f], |g| = %9.2e, |p| = %7.2e, Delta = %5.2f \n',...
		 n,x(1),x(2),norm(g(x)), norm(p), Delta);
   Delta = DeltaNew;
   pause
end  

hold off


function [p,mp] = cauchypoint(g,B,Delta)
% Find the Cauchy point,
% see 4.11, 4.12 in N&W. 
gBg = g'*B*g;
g2 = g'*g; 
alpha = g2/gBg;
pn = alpha*norm(g);
if gBg > 0 & pn < Delta
    p = -alpha*g;
else    
    p = -Delta*g/norm(g);
end
mp = g'*p+0.5*p'*B*p;



function [p,mp] = dogleg(g,B,Delta)
% The dogleg method

if min(eig(B)) <= 0
    error('B is not SPD, the dogleg method can not be used');
end
gBg = g'*B*g;
g2 = g'*g; 
alpha = g2/gBg;
pU = -alpha*g;
pFS = -B\g;
if norm(pU) >= Delta
    p = -Delta*g/norm(g);
elseif norm(pFS) <= Delta 
    p = pFS; 
else
    dp = pFS-pU;
    a = dp'*dp;
    b = 2*pU'*dp;
    c = pU'*pU-Delta^2;
    r = roots([a,b,c]);
    tau = r(r>=0 & r <= 1)+1;
    p = pU + (tau-1)*(pFS-pU);
end
mp = g'*p+0.5*p'*B*p;

















