% This codes solves the Generalized Trust Region Subproblem:
%
% min x'Ax - 2a'x   s.t.   x'Bx = s
% where B is positive definite
%
% It solves the dual problem
% max d(lambda) = -s lambda + a'(A - lambda B)^{-1}a s.t. lambda < min(eig(A,B))
% by Newton's method with Armijo line search rule. The Newton direction is
% obtained via pcg.
% 
% Input
% A, a, B, s - problem data input
% gtol       - termination tolerance
% maxiter    - maximum allowed iterations
%
% Output
% u      - approximate primal optimal solution: (A - lambda B)^{-1}a.
% lambda - approximate dual optimal solution
% pval   - approximate primal optimal value
% feas   - approximate primal feasibility: u'Bu - s
% count  - number of iterations
% t_final- total CPU time
% dval   - approximate dual optimal value
%
% Subroutines called: Ahandle, Bhandle, eigifp
%
% Written by Ting Kei Pong and Henry Wolkowicz
% Last updated on Sep 21, 2012

function [u,lambda,pval,feas,count,t_final,dval] = Newton_gtrs(A,a,B,s,gtol,maxiter)
tstart = tic;

global flag_shiftA
global A0
global B0
global MVcount

flag_shiftA = 0;
MVcount = 0;
A0 = A;
B0 = B;

Ahandle0 = @NLfun;
Bhandle0 = @NBfun;

nA = normest(A,1);
na = norm(a);


opts.useprecon = 'NO';
opts.disp = 0;
opts.initialvec = sum(A)';
opts.tol = 1e-8;
opts.maxit = 10000;
lambda0 = eigifp('Ahandle','Bhandle',1,opts);
t2 = toc(tstart);
% fprintf(' time for finding min eig: %g\n', t2)
fprintf('\n')

tol = 1e-14; % for pcg

lambda = lambda0 - 1; % heuristics
lambda1 = lambda;
[u,dummy] = pcg(Ahandle0,a,tol,1000);
Bub = Bhandle0(u);
dslope = -s + u'*Bub;
dval = -s*lambda + a'*u;
lambda1 = 0;
Au = Ahandle0(u);
pval = u'*Au - 2*a'*u;
optcon = norm(Au - lambda*Bub - a)/(nA + na + 1);
count = 0;
tmpdir = u;

while ~(abs(dslope)/(abs(s) + 1) < gtol && abs(pval + dval)/(abs(pval) + 1) < gtol && optcon < sqrt(gtol)) && count < maxiter
  
  count = count + 1;
  % compute Hessian
  lambda1 = lambda;
  [tmpdir,dummy] = pcg(Ahandle0,Bub,tol,1000,[],[],tmpdir);
  hess = 2*Bub'*tmpdir;
  dir = -dslope/hess;
  step = 1;
  if dir > 0
    step = min(1,0.95*(lambda0 - lambda)/dir); % backtrack
  end
  dderiv = dir*dslope;
  
  % Armijo rule
  lambdanew = lambda + step*dir;
  lambda1 = lambdanew;
  [unew,dummy] = pcg(Ahandle0,a,tol,1000,[],[],u);
  dnew = -s*lambdanew + a'*unew;
  while dnew > dval + 1e-4*step*dderiv && step > 1e-10 && dderiv < 0
    step = step/2;
    lambdanew = lambda + step*dir;
    lambda1 = lambdanew;
    [unew,dummy] = pcg(Ahandle0,a,tol,1000,[],[],unew);
    dnew = -s*lambdanew + a'*unew;
  end
  
  if step <= 1e-10 || dderiv >= 0
    fprintf(' Early termination: stepsize %g dir %g', step, dderiv)
    break
  end
  
  lambda = lambdanew;
  u = unew;
  dval = dnew;
  
  Bub = Bhandle0(u);
  dslope = -s + u'*Bub;
  lambda1 = 0;
  Au = Ahandle0(u);
  pval = u'*Au - 2*a'*u;
  optcon = norm(Au - lambda*Bub - a)/(nA + na + 1);
  
%   fprintf(' iter %4d    fval %16.10f   %16.10f   feas %16.10f  step %g\n', count, pval, -dval, u'*B*u - s, step)
  
end

feas = u'*Bhandle0(u) - s;

t_final = toc(tstart);

fprintf('\n ------------- Newton Results -------------\n')

fprintf(' Primal value       %16.10f  Dual value       %16.10f\n', pval, -dval)
fprintf(' Gap                %16.10f  Feasibility      %16.10f\n', pval + dval, feas)
fprintf(' CPU time           %16.10f  Rel Opt Cond.    %16.10f\n', t_final, optcon)
fprintf(' Number of MV       %16.10f\n', MVcount)

% Nested function

function v = NLfun(x)
    
    if lambda1 ~= 0
      v = A*x - lambda1*(B*x);
      MVcount = MVcount + 2;
    else
      v = A*x;
      MVcount = MVcount + 1;
    end
    
end

function v = NBfun(x)
    
    v = B*x;
    MVcount = MVcount + 1;
    
end

end

