function [p,at_bdry] = solve_tr(g,B,Delta,tol,trace)
% [p,at_bdry] = solve_tr(g,B,Delta,tol,trace)
% Solve trust region problem
%
%    min_p g'*p + (1/2)p'*B*p,	||p||_2 \le \Delta
%
% for symmetric B
%
% Uses bracketing approach to find appropriate lambda where
% p = -(B+\lambda I)^{-1}g.
%
% tol is the tolerance for lambda
% If trace is not zero, then print out information about the process.

% Check input data
n = length(g);
[nrows,ncols] = size(B);
if nrows ~= n | ncols ~= n
  fprintf('solve_tr: incompatible sizes\n')
  p = zero(n,1);
  at_bdry = 0;
  return
end
if Delta <= 0
  fprintf('solve_tr: Delta must be positive\n');
  p = zero(n,1);
  at_bdry = 0;
  return
end

% Deal with trivial cases first
normB = norm(B,'fro');
if normB == 0.0
  if norm(g) == 0
    p = zero(n,1);
    at_bdry = 0;
  else
    p = -g/norm(g,2)*Delta;
    at_bdry = 1;
  end
  return
end


% Compute Geshgorin bound for lambda1
lambda1 = 0;
for i = 1:n
  temp = B(i,i) - sum(abs(B(i,1:(i-1)))) - sum(abs(B(i,(i+1):n)));
  if temp < lambda1
    lambda1 = temp;
  end
end
if trace ~= 0
  fprintf('solve_tr: lambda_1(B) = %g, Gershgorin bound = %g\n', ...
      min(eig(B)), lambda1);
end

lambda_hi = norm(g,2)/Delta - lambda1;
lambda_lo = 0;
lambda_old = 0;
at_bdry = 1;

while abs(lambda_lo-lambda_hi) > tol*normB
  if trace ~= 0
    fprintf('solve_tr: lambda_lo = %g, lambda_hi = %g\n', ...
	lambda_lo,lambda_hi);
  end
  C = B+lambda_lo*eye(n);
  [R,cholflag] = chol(C);
  if cholflag ~= 0
    % Cholesky factorization failed
    if trace ~=0
      fprintf('solve_tr: Cholesky factorization failed\n');
    end
    lambda_old = lambda_lo;
    lambda_lo  = (lambda_lo+lambda_hi)/2;
  else
    p = -C\g;
    if trace ~= 0
      fprintf('solve_tr: ||p|| = %g, Delta = %g\n', norm(p,2), Delta);
    end
    if norm(p,2) < Delta
      lambda_hi = lambda_lo;
      lambda_lo = (lambda_lo+lambda_old)/2;
    else
      if norm(p,2) == Delta
	lambda_hi = lambda_lo;
      end
      break
    end % if...else
  end % if...else
end % while

if lambda_hi == 0
  at_bdry = 0;
  return
end

% If cholflag ~= 0 then we can't factor B+lambda_lo*eye(n) but we
% can factor B+lambda_hi*eye(n) even through |lambda_hi-lambda_lo| < tol*normB
if cholflag ~= 0
  if trace ~= 0
    fprintf('solve_tr: Can''t factor B+lambda_lo*I\n');
  end
  C = B + lambda_hi*eye(n);
  p = -C\g;
  at_bdry = 1;
  return
end

if trace ~= 0
  fprintf('solve_tr: Now for part 2\n')
end

% Now apply Newton/bracketing method to solve ||p||_2 = Delta
lambda_old = lambda_lo;
while abs(lambda_lo-lambda_hi) > tol*normB
  if trace ~= 0
    fprintf('solve_tr: lambda_lo = %g, lambda_hi = %g\n', ...
	lambda_lo,lambda_hi);
  end
  if abs(norm(p,2)-Delta) < tol*Delta
    at_bdry = 1;
    return
  end
  % Use ideas in "TR-exact" algorithm, Nocedal & Wright, p. 84
  q = R'\p;
  lambda = lambda_old + (p'*p)/(q'*q)*(norm(p,2)-Delta)/Delta;
%  if lambda <= lambda_lo | lambda >= lambda_hi
%    if trace ~= 0
%      fprintf('solve_tr: use bisection\n');
%    end
%    lambda = (lambda_lo+lambda_hi)/2;
%  end
  if lambda <= lambda_lo+0.1*(lambda_hi-lambda_lo)
    if trace ~= 0
      fprintf('solve_tr: lambda in extreme left-hand part of interval\n');
    end
    lambda = lambda_lo+0.1*(lambda_hi-lambda_lo);
  elseif lambda >= lambda_hi-0.1*(lambda_hi-lambda_lo)
    if trace ~= 0
      fprintf('solve_tr: lambda in extreme right-hand part of interval\n');
    end
    lambda = lambda_hi-0.1*(lambda_hi-lambda_lo);
  end 
  
  % Use lambda as our new guess
  C = B+lambda*eye(n);
  R = chol(C);
  p = -C\g;
  if trace ~= 0
    fprintf('solve_tr: ||p|| = %g, lambda = %20.12g\n', ...
	norm(p,2),lambda);
  end
  lambda_old = lambda;
  if norm(p,2) < Delta
    lambda_hi = lambda;
  else
    lambda_lo = lambda;
  end % if...else
end % while
if norm(p,2) < 0.99*Delta
  C = B+lambda_hi*eye(n);
  p = -C\g;
end