% This codes solves the Generalized Trust Region Subproblem:
%
% min x'Ax - 2a'x   s.t.   lb <= x'Bx <= ub
% where B is positive definite
%
% The code starts with preprocessing that either solves the problem by 
% returning an interior solution, an explicit solution in hard case 2
% instances via shift and deflation, or reduces the problem to an
% equivalent equality constrained problem, i.e., x'Bx = s.
%
% The equality constrained problem is further reformulated into
%                  max k(t) := k0(t) - t
% where k0(t) = inf{y'[t -a';-a A]y: y'[1 0;0 B]y = s + 1},
% and solved via the extended Rendl Wolkowicz algorithm.
%
% Input
% 
% A, B, a, lb, ub - input data from the problem
% gtol            - termination tolerance
%
% Output (argout in order)
%
% y       - approximate optimal solution
% iter    - number of iterations to solve the equality constrained problem
% fval    - approximate primal optimal value: y'Ay - 2a'y
% feas    - approximate primal infeasibility: y'By - min(max(y'By,lb),ub)
% dval    - approximate dual optimal value: k(t)
% k_slope - slope of k at termination
% t_final - total CPU time
% lambda  - approximate optimal Lagrange multiplier.
% s       - the equality constraint that is attained. Equals 0 for interior
%           solution
%
% Subroutines called: Ahandle, Bhandle, Chandle, Dhandle, eigifp
%
% Written by Ting Kei Pong and Henry Wolkowicz
% Last updated on Sep 21, 2012


function varargout = gtrsalgo_ul(A,a,B,lb,ub,gtol)

tstart = tic;

global A0
global B0
global a0
global tt
global flag_shiftA
global lambdamin
global alphall 
global valb
global MVcount

fprintf(' ************** START ***************\n')
A0 = A;
B0 = B;
a0 = a;
MVcount = 0; % record matrix vector multiplications
n = size(A,1);
zerotol = 1e-8; % works well in simulations

%% case check
opts.size = n; % options for eigifp
opts.useprecon = 'NO';
opts.disp = 0;

tcheck = tic;
[s,flag_case,output,lambda0,v0] = GTRS_case_check(A,a,lb,ub,zerotol);
fprintf(' time for checking hard cases = %g; MV used here = %d\n', toc(tcheck), MVcount)

if flag_case ~= 0 %  interior solution or hardcase 2
  t_final = toc(tstart);
  
  fprintf('\n ------------- Results -------------\n')

  y = output.y;
  fval = output.fval;
  feas = output.feas;
  lopt = output.lopt;
  fprintf(' Primal value       %16.10f  Feasibility      %16.10f\n', fval, feas)
  fprintf(' Rel Opt Cond.      %16.10f  CPU time         %16.10f\n', norm(A*y - a - lopt*(B*y))/(nA + na + 1), t_final)
  fprintf(' Number of MV       %16.10f\n', MVcount)
  
  if nargout >= 1; varargout{1} = y; end
  if nargout >= 2; varargout{2} = 0; end
  if nargout >= 3; varargout{3} = fval; end
  if nargout >= 4; varargout{4} = feas; end
  if nargout >= 5; varargout{5} = fval; end
  if nargout >= 6; varargout{6} = 0; end
  if nargout >= 7; varargout{7} = t_final; end
  if nargout >= 8; varargout{8} = lopt; end
  if nargout >= 9; varargout{9} = s; end
  return
end

if flag_shiftA > 0 
  flag_shiftA = 2;
end

% end of case check
fprintf(' ub = %g, lb = %g, s = %g\n', ub, lb, s)

s1 = abs(s + 1);
ss1 = sqrt(s1);
fprintf(' Problem size: %d, s: %16.10f\n', n,s);

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

%% maximize k(t)
tinit = tic;

[invBa,dummy] = pcg('Bhandle',a,1e-14,1000);
ainvBa = a'*invBa;
low = lambda0 - sqrt(ainvBa/s);
high = lambda0 + sqrt(s*ainvBa);

fprintf(' time for initialization = %g\n', toc(tinit))
fprintf(' Initial end points: %16.10f  %16.10f\n', low, high)

v_init = [1;v0]; % [0;v0] does not work for hard case

opts.size = n + 1; % options for eigifp
opts.maxit = 5000;

side = 0;
iter = 0;

gap = inf;
fbest = inf;
feas = inf;
optcon = inf;
k_slope = 1;
ptgood = []; ptbad = [];

%% Enter main loop
fprintf(' ------------- Enter main loop -------------\n')

while ~((abs(k_slope)/s1 < sqrt(gtol) && abs(gap)/(abs(fbest) + 1) < gtol && abs(feas)/(abs(s) + 1) < gtol && optcon < sqrt(gtol)) || (high - low)/(abs(high) + abs(low)) < gtol/100) && iter < 30
    
  iter = iter + 1;
  
  gindex = size(ptgood,1);
  bindex = size(ptbad,1);
  
  % strategies for maximizing k
  
  tt = (high + low)/2; % midpoint rule  
  
  [tt,high,low] = vertical_cut(gindex,bindex,ptgood,ptbad,high,low,tt); % vertical cut; update both t and the interval, if possible
  
  tt = trig_interpol(gindex,bindex,ptgood,ptbad,high,low,tt); % triangle interpolation; overwrites the previous only when triangle lies between low and high
  
  tt = inv_interpol(gindex,bindex,ptgood,ptbad,side,ss1,high,low,tt); % inverse interpolation; overwrites the previous only when inv lies between low and high

  eigtol = max(min(abs(k_slope)*1e-4,(high - low)/(abs(high) + abs(low))),zerotol); % approximate eig, has to be fairly accurate
%   eigtol = zerotol; % approx eig fails when abs(s) is large
  if side > 0
    v_init = gv_init;
  elseif side < 0
    v_init = bv_init;
  end
  
  opts.initialvec = v_init; % warm start may make eigifp get stuck
  opts.tol = eigtol;
  [v, v_init, lambdaout] = GTRS_ktsubproblem('Chandle','Dhandle',opts);
  
  k_slope = s1*v(1)^2 - 1;  
  dval = s1*lambdaout - tt;
  
  % record good side/bad side points
  
  if k_slope > 0
    
    ptbad = [tt abs(v(1)) k_slope dval;ptbad];
    low = tt;
    side = min(-1,side - 1);
    bv = v;
    bv_init = v_init;
    
    if gindex > 0 % primal step to the boundary

      vf = v(2:end)/v(1);
      if abs(gv(1)) < zerotol 
        z = gv(2:end); % take a step with smaller magnitude
        fprintf(' warning: first entry small \n')
      else
        z = gv(2:end)/gv(1) - vf; % take a step between 0 and 1
      end
      Bz = Bhandle(z);
      zBz = z'*Bz;
      xyprod = Bz'*vf;  % normalized inner product
      ndiff = s - vf'*Bhandle(vf);
      sxy = sign(xyprod);
      if sxy == 0,
        sxy = 1;
      end
      tau = ndiff/(xyprod + sxy*sqrt(xyprod^2 + zBz*ndiff));
      vnew = vf + tau*z;
      fval = vnew'*Ahandle(vnew) - 2*a'*vnew;
      if fbest >= fval
        y = vnew;
        fbest = fval;
      end
      gap = fbest - dval;
      Bytmp = Bhandle(y);
      feas = y'*Bytmp - s;
      optcon = norm(Ahandle(y) - lambdaout*Bytmp - a)/(nA + na + 1);
      fprintf('%3d bad : shifted primal value %6.5e, gap %3.2e, feas %3.2e\n', iter, fbest, gap, feas) 
      
    end
    
  elseif k_slope < 0
    
    ptgood = [tt abs(v(1)) k_slope dval;ptgood];
    high = tt;
    side = max(1,side + 1);
    gv = v;
    gv_init = v_init;
    
    if bindex > 0 % primal step to the boundary
      
      vf = bv(2:end)/bv(1);
      if abs(v(1)) < zerotol
        z = v(2:end); % take a step with smaller magnitude
        fprintf(' warning: first entry small \n')
      else
        z = v(2:end)/v(1) - vf; % take a step between 0 and 1
      end
      Bz = Bhandle(z);
      zBz = z'*Bz;
      xyprod = Bz'*vf;  % normalized inner product
      ndiff = s - vf'*Bhandle(vf);
      sxy = sign(xyprod);
      if sxy == 0,
        sxy = 1;
      end
      tau = ndiff/(xyprod + sxy*sqrt(xyprod^2 + zBz*ndiff));
      vnew = vf + tau*z;
      fval = vnew'*Ahandle(vnew) - 2*a'*vnew;
      if fbest >= fval
        y = vnew;
        fbest = fval;
      end
      gap = fbest - dval;
      Bytmp = Bhandle(y);
      feas = y'*Bytmp - s;
      optcon = norm(Ahandle(y) - lambdaout*Bytmp - a)/(nA + na + 1);
      fprintf('%3d good: shifted primal value %6.5e, gap %3.2e, feas %3.2e\n', iter, fbest, gap, feas) 
      
    end
    
  end

end

fval = y'*Ahandle(y) - 2*a'*y;
lambda = lambdaout;
dval = (s + 1)*lambda - tt;
constraint = y'*Bhandle(y);
feas = constraint - max(min(constraint,ub),lb);
if flag_shiftA > 0
  fval = fval + lambdamin*s;
  dval = dval + lambdamin*s;
  lambda = lambda + lambdamin;
end
t_final = toc(tstart);

% print output

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

fprintf(' Primal value       %16.10f  Dual value       %16.10f\n', fval, dval)
fprintf(' Gap                %16.10f  Feasibility      %16.10f\n', gap, feas)
fprintf(' interval width     %16.10f  Rel Opt Cond.    %16.10f\n', high - low, optcon)
fprintf(' k_slope (approx)   %16.10f  CPU time         %16.10f\n', k_slope, t_final)
fprintf(' Number of MV       %16.10f\n', MVcount)

if nargout >= 1; varargout{1} = y; end
if nargout >= 2; varargout{2} = iter; end
if nargout >= 3; varargout{3} = fval; end
if nargout >= 4; varargout{4} = feas; end
if nargout >= 5; varargout{5} = dval; end
if nargout >= 6; varargout{6} = k_slope; end
if nargout >= 7; varargout{7} = t_final; end
if nargout >= 8; varargout{8} = lambda; end
if nargout >= 9; varargout{9} = s; end

%% end of main function




%% nested functions

function [s,flag_case,output,lambda0,v0] = GTRS_case_check(A,a,lb,ub,zerotol)

global vall

flag_shiftA = 0;          % flag_shiftA will be changed to 1 if we have the possible hard case
flag_posshc1 = 0;

% Calculate the smallest eigenvalue and eigenvector of the matrix pencil (A,B)
opts.initialvec = sum(A)';
opts.tol = zerotol;
opts.maxit = 10000;
[lambda,va] = eigifp('Ahandle','Bhandle',1,opts);
va = va/norm(va);    % normalize
nA = normest(A,1);
na = norm(a);
n = size(A,1);
tol = 1e-14; % for pcg

lambda0 = lambda;
v0 = va;

if norm(a) < zerotol % check for early termination if a is small      
                     % The problem is just a generalized eigenvalue problem    
  if lambda > 0 
    xopt = sqrt(lb)*va;   
    lopt = lambda;
    s = lb;
  else      
    xopt = sqrt(ub)*va;
    lopt = lambda;
    s = ub;
  end
  fopt = xopt'*Ahandle(xopt) - 2*a'*xopt;                

  flag_case = 1;
  output.y = xopt;
  output.fval = fopt;
  output.lopt = lopt;
  constraint = xopt'*Bhandle(xopt);
  output.feas = constraint - max(min(constraint,ub),lb);
  return    
end

lambdamin = lambda; % save the smallest eigenvalue of A
vaa = va'*a/na;
if vaa < 0 % loop to ensure the inner product is positive
  va = -va;
  vaa = -vaa;
end
    
if lambdamin <= 0
  
  flag_kcase = 1; % solve k(t) with s = ub;
  
  if vaa < zerotol % We have the 'almost' hard case
  % try to take advantage of hard case, case 2 to solve for opt explicity using only pcg
    flag_shiftA = 1;  % indicates a shift by lambda*B for Ahandle only
    flag_posshc2 = 1;    % indicates possible hard case, case 2          

       %% save values for the deflation if multiple min eigenvalue
    alphall = []; vall = []; valb = [];  lambdall = [];       % save alpha, va, lambda
    nAn = nA/sqrt(n); % nA=||A||_1, the value used for the deflation

    while flag_posshc2  % while lambdamin is still 0 and we still have the possible hard case (case 2)
      if vaa < zerotol   % indicates possible hard case, case 2
        alphall = [alphall; nAn];  % save the value of the alpha
        vall = [vall, va];   % save the value of va, eigenvector, ensure norm 1
        valb = [valb, Bhandle(va)];
        lambdall = [lambdall, lambda]; % save the value of lambda
        opts.initialvec = randn(n,1);
        eigtime = tic;
        [lambda,va] = eigifp('Ahandle',1,opts); % Compute the smallest eigenvalue after shift and deflation
        fprintf(' eig diff = %g, time for eigifp = %g\n', norm(Ahandle(va) - lambda*va), toc(eigtime))
    		va = va/norm(va);    % normalize

        vaa = va'*a/na;
        if vaa < 0 % loop to ensure the inner product is positive
          va = -va;
          vaa = -vaa;
        end
               
      else
        flag_posshc2 = 0;            
      end
      
      if lambda >= zerotol % if the vector just found is no longer in the nullspace of A - lambdamin*B
        break
      end
    end
    
    if flag_posshc2 == 1 % possible hard case, case 1
      flag_posshc1 = 1;
    end

    if flag_posshc2,   % possible hard case, case 2
      disp('**POSSIBLE** hard case, case 2; using pcg to find x'); 

      [xtemp, dummy] = pcg('Ahandle',a,tol,1000);  % solve the x(\lambda)
      fprintf(' pcg residual %g\n', norm(Ahandle(xtemp) - a))

      Bxtemp = Bhandle(xtemp);
      tau2 = ub - xtemp'*Bxtemp;  % compute tau - step length to boundary            

      if tau2 >= 0,                  % we do have the hard case
	      disp('HARD CASE, CASE 2; taking step to upper boundary');
        vb = vall(:,1)/sqrt(vall(:,1)'*valb(:,1));
        b = vb'*Bxtemp;
        c = -tau2;
        % quadratic formula
        if b < 0
          stepsize = -b + sqrt(b^2 - c);
        else
          stepsize = c/(-b - sqrt(b^2 - c));
        end
        fprintf(' stepsize = %g\n', stepsize)
        
        xopt = xtemp + stepsize*vb;
        lopt = lambdamin;
        flag_shiftA = 0;
        fopt = xopt'*Ahandle(xopt) - 2*a'*xopt;
               
        flag_case = 1;
        output.y = xopt;
        output.fval = fopt;
        output.lopt = lopt;
        constraint = xopt'*Bhandle(xopt);
        output.feas = constraint - max(min(constraint,ub),lb);
        s = ub;
        return               
      else  % x(\lambda) > s, not the hard case (2)
        flag_posshc2 = 0;
      end
    end
  end % if lambdamin < zerotol && vaa < zerotol % We have the 'almost' hard case
    
else % lambdamin > 0
    
  [xtemp, dummy] = pcg('Ahandle',a,tol,1000);  % solve for possible interior solution
  fprintf(' pcg residual %g\n', norm(Ahandle(xtemp) - a))
  Bxtemp = Bhandle(xtemp);
  tau1 = xtemp'*Bxtemp - lb;
  tau2 = ub - xtemp'*Bxtemp;
    
  if tau1 >=0 && tau2 >= 0 % interior solution found
      
    fprintf('** Interior solution\n');
    xopt = xtemp;
    lopt = 0;        
    fopt = xopt'*Ahandle(xopt) - 2*a'*xopt;
               
    flag_case = 2; % interior solution
    output.y = xopt;
    output.fval = fopt;
    output.lopt = lopt;
    constraint = xopt'*Bhandle(xopt);
    output.feas = constraint - max(min(constraint,ub),lb);
    s = 0;
    return            
  end
    
  if tau1 < 0; flag_kcase = 0; end % maximize k(t) for s = lb
  if tau2 < 0; flag_kcase = 1; end % maximize k(t) for s = ub

  if vaa < zerotol % We have the 'almost' hard case
    % try to take advantage of hard case, case 2 to solve for opt explicity using only pcg
    flag_shiftA = 1;  % indicates a shift by lambda*B for Ahandle only
    flag_posshc2 = 1;    % indicates possible hard case, case 2       

       %% save values for the deflation if multiple min eigenvalue
    alphall = []; vall = []; valb = [];  lambdall = [];       % save alpha, va, lambda
    nAn = nA/sqrt(n); % nA=||A||_1, the value used for the deflation

    while flag_posshc2,  
      % while we still have the possible hard case (case 2)
      if vaa < zerotol,   % indicates possible hard case, case 2
        alphall = [alphall; nAn];  % save the value of the alpha
        vall = [vall, va];   % save the value of va, eigenvector, ensure norm 1
        valb = [valb, Bhandle(va)];
        lambdall = [lambdall, lambda]; % save the value of lambda
        opts.initialvec = randn(n,1);
        eigtime = tic;
        [lambda,va] = eigifp('Ahandle',1,opts); % Compute the smallest eigenvalue after shift and deflation
        fprintf(' eig diff = %g, time = %g\n', norm(Ahandle(va) - lambda*va), toc(eigtime))
        va = va/norm(va);     % scaling

        vaa = va'*a/na;
        if vaa < 0 % loop to ensure the inner product is positive
          va = -va;
          vaa = -vaa;
        end 
               
      else
        flag_posshc2 = 0;            
      end 
     
      if lambda >= zerotol
        break
      end
    end
    
    if flag_posshc2 == 1 % possible hard case, case 1
      flag_posshc1 = 1;
    end

    if tau1 < 0 % hard case 2 can only occur towards lb
      if flag_posshc2   % possible hard case, case 2 ... check norm of x found using pcg
        disp('**POSSIBLE** hard case, case 2; using pcg to find x');

        [xtemp, dummy] = pcg('Ahandle',a,tol,1000);  % solve the x(\lambda) 
        fprintf(' pcg diff = %g\n', norm(Ahandle(xtemp) - a))
        Bxtemp = Bhandle(xtemp);
        tau1 = xtemp'*Bxtemp - lb; % compute tau - step length to boundary      

        if tau1 <= 0,                  % we do have the hard case
	        disp('HARD CASE, CASE 2; taking step to lower boundary');
          vb = vall(:,1)/sqrt(vall(:,1)'*valb(:,1));
          b = vb'*Bxtemp;
          c = tau1;
          if b < 0
            stepsize = -b + sqrt(b^2 - c);
          else
            stepsize = c/(-b - sqrt(b^2 - c));
          end
          xopt = xtemp + stepsize*vb;
          lopt = lambdamin;
          flag_shiftA = 0;
          fopt = xopt'*Ahandle(xopt) - 2*a'*xopt;
               
          flag_case = 1;
          output.y = xopt;
          output.fval = fopt;
          output.lopt = lopt;
          constraint = xopt'*Bhandle(xopt);
          output.feas = constraint - max(min(constraint,ub),lb);
          s = lb;
          return 
        else                   
          flag_posshc2 = 0;
        end
               
      else  % x(\lambda) > s, not the hard case (2)
        flag_posshc2 = 0;
      end
    end        
  end % if lambdamin < zerotol && vaa < zerotol % We have the 'almost' hard case   
end     

flag_case = 0;
flag_shiftA = 0;
output = 0;
if flag_posshc1 == 1
  flag_shiftA = 2;
  opts.initialvec = randn(n,1);
  eigtime = tic;
  [lambda,va] = eigifp('Ahandle',1,opts); % Compute the smallest eigenvalue after shift and deflation
  fprintf(' posshc1: recompute eig:\n eig diff = %g, time for eigifp = %g\n', norm(Ahandle(va) - lambda*va), toc(eigtime))
  lambda0 = lambda;
  v0 = va/norm(va);
end

% maximize k(t) with s = ub or lb, according to flag_kcase
if flag_kcase == 0
  s = lb;
else
  s = ub;
end

end

function [v,v_init,dummy] = GTRS_ktsubproblem(C,D,opts)

% Solves
%
% min x'*C*x s.t. x'*D*x = 1
%
% Assumes D > 0.

[dummy,v] = eigifp(C,D,1,opts);
v_init = v;
% normDv = sqrt(v'*feval(D,v));
% v = v/normDv;

end

function tmid = trig_interpol(gindex,bindex,ptgood,ptbad,high,low,tmid)

if bindex > 0 && gindex > 0
  
  % k'(t) from the last point on either side
  tbslope = ptbad(1,3);
  tgslope = ptgood(1,3); 
  
  % k(t) from the last point on either side
  ktb = ptbad(1,4);
  ktg = ptgood(1,4);
  
  ttemp = (tbslope*ptbad(1,1) + ktg - tgslope*ptgood(1,1) - ktb)/(tbslope - tgslope);
  
  if low <= ttemp && ttemp <= high
    tmid = ttemp;
%     fprintf('triang used\n')
  else
%     fprintf('triang fails: outside\n')
  end
end

end

function [tmid,high,low] = vertical_cut(gindex,bindex,ptgood,ptbad,high,low,tmid)

if gindex > 0 && bindex > 0
  
  ktg = ptgood(1,4);
  ktb = ptbad(1,4);
  
  if ktg > ktb % value on good side > bad side
    tmplow = ptbad(1,1) + (ktg - ktb)/ptbad(1,3);
    
    if low <= tmplow && tmplow <= high
      low = tmplow;
%       fprintf(' vertical cut: interval reduced\n')
      if tmid < low
        tmid = low;
%         fprintf(' vertical cut used\n')
      end
    end
  else
    tmphigh = ptgood(1,1) + (ktb - ktg)/ptgood(1,3);
    
    if low <= tmphigh && tmphigh <= high
      high = tmphigh;
%       fprintf(' vertical cut: interval reduced\n')
      if tmid > high
        tmid = high;
%         fprintf(' vertical cut used\n')
      end
    end
  end
end

end

function tmid = inv_interpol(gindex,bindex,ptgood,ptbad,side,ss1,high,low,tmid)

if side < 0 && bindex > 1 % inverse interpolation
  tint = ptbad(1:2,1); 
  vint = ptbad(1:2,2);
  
  f1 = -1/vint(1) + ss1; 
  f2 = -1/vint(2) + ss1;
    
  slope = (tint(1) - tint(2))/(f1 - f2);
  ttemp = tint(1) - f1*slope; % linear inverse interpolation
  if (low <= ttemp) && (ttemp <= high)
    tmid = ttemp;
%     fprintf(' inv interpol used\n')
  else
%     fprintf(' inv interpol failed\n')
  end
else
  if side > 0 && gindex > 1 % inverse interpolation
    tint = ptgood(1:2,1); 
    vint = ptgood(1:2,2);
    
    f1 = -1/vint(1) + ss1; 
    f2 = -1/vint(2) + ss1;
  
    slope = (tint(1) - tint(2))/(f1 - f2);
    ttemp = tint(1) - f1*slope; % linear inverse interpolation
    if (low <= ttemp) && (ttemp <= high)
      tmid = ttemp;
%       fprintf(' inv interpol used\n')
    else
%       fprintf(' inv interpol failed\n')
    end
  end
end

end

end