% This codes solves the Generalized Trust Region Subproblem:
%
% min x'Ax - 2a'x   s.t.   x'Bx = s
% where B is indefinite nonsingular, A is positive definite
%
% It solves the dual problem
% max d(lambda) = -s lambda + a'(A - lambda B)^{-1}a s.t. underline{lambda} < lambda < overline{lambda}
% by Newton's method with Armijo line search rule. The Newton direction is
% obtained via pcg. The underline and overline lambdas are computed using
% eigs, and they are the largest negative and smallest positive generalized
% eigenvalue of the pencil (A,B), respectively.
% 
% 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
%
% 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_indB(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);

% fprintf(' --------------- Initialization -------------\n')

n = size(A,1);
time = clock;
opts.useprecon = 'NO';
opts.disp = 0;
opts.maxit = 10000;
opts.initialvec = sum(A)';
opts.tol = 1e-8;
opts.size = n;

% Calculate the first positive and negative eigenvalues, with the corresponding eigenvectors, of the matrix pencil (A,B)
mu = eigifp('nBhandle','Ahandle',1,opts);
bb = -1/mu;
mu = eigifp('Bhandle','Ahandle',1,opts);
aa = 1/mu;
t2 = etime(clock,time);
% fprintf(' time for finding interval = %g\n', t2)

fprintf('\n')
tol = 1e-14; % for pcg
bb0 = bb;
aa0 = aa;

lambda = (aa + bb)/2;
lambda1 = lambda; % input for Ahandle0
[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;
  if dir > 0
    step = min(1,0.95*(bb0 - lambda)/dir);
  else step = min(1,0.95*(lambda - aa0)/(-dir));
  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, dslope, step) 
  
end

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

t_final = toc(tstart);

fprintf('\n ------------- 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
  