function trustregion_newtonCG(n)
% Trust region method with Newton CG 
% n = problem dimension

x0 = zeros(n,1);
xE = ones(n,1); % Exact

% Trust-region algorithm
Delta = 1.0;
xk    = x0;

[fk, gk, Hk] = rosenbrock(xk);
g0 = gk;

Nmax  = 1000;

fprintf('%3d: f=%10.3e, |df|=%10.3e, |x-x*| = %10.3e, Delta = %10.3e\n', ...
            0, fk, norm(g0), norm(xk-xE), Delta);

for k = 1:Nmax
       
    if norm(gk) <= 1.0E-12*norm(g0)
        % converged!
        break;
    end
    
    [pk status_cg] = cg_steinhaug(n, fk, gk, Hk, Delta);
    
    fk1 = rosenbrock(xk+pk);
    
    %  check the actual reduction
    rho = (fk - fk1) / (-gk'*pk - pk'*Hk*pk/2);
    % update the trust-region radius
    if rho < 0.25
        Delta = 0.25*Delta;
    elseif rho > 0.75 && Delta-norm(pk) >= -1.0E-06*Delta
        Delta = min(2*Delta, 2.0);
    end
    % accept the step, if the ratio is OK
    if rho > 1.0E-03
        xk = xk+pk;
    end
    
    [fk gk Hk] = rosenbrock(xk);
    
    fprintf('%3d: f=%10.3e, |df|=%10.3e, |x-x*| = %10.3e, Delta = %10.3e, CG status: %s\n', ...
            k, fk, norm(gk), norm(xk-xE), Delta, status_cg);
end

end


function [p status] = cg_steinhaug(n, f, g, H, Delta)
% Algorithm 7.2 in N&W
eps = 1e-10;
Nmax = 10000;
z = zeros(n,1);
r = g;
d = -r;
if (norm(r) < eps) 
    p = z;
    status = 'Stopping criteria';
    return
end
for i=1:Nmax
   if (d'*H*d <= 0)
       status = 'Negative curvature';
       % Solve ||z + tau d|| = Delta
       tau = roots([d'*d, d'*z, z'*z - Delta^2]);
       p1 = z + tau(1)*d; % first candidate
       p2 = z + tau(2)*d; % second candidate
       % Check which is largest
       m1 = f + g'*p1 + p1'*H*p1/2;
       m2 = f + g'*p2 + p2'*H*p2/2;
       if (m1 < m2)
           p = p1;
           return
       else
           p = p2;
           return
       end
   end
   a = (r'*r) / (d'*H*d);
   zo = z;
   z = z + a*d;
   if (norm(z) >= Delta)
       % Solve ||z + tau d|| = Delta
       tau = max(roots([d'*d, d'*zo, zo'*zo - Delta^2]));
       assert (tau >= 0) % A positive solution should exist
       p = zo + tau*d;
       status = 'Trust-region boundary';
       return
   end
   ro = r;
   r = r + a*H*d;
   if (norm(r) < eps)
       p = z;
       status = 'Stopping criteria';
       return
   end
   b = (r'*r) / (ro'*ro);
   d = -r + b*d;
end

% Should not end up here (unless this method does'nt converge)
cerr('CG-Steinhaug did not converge');

end