function [p,pBp] = cgsteihaug(fnhess,fngrad,fn,gradold,xold)
% usage: [p,pBp] = cgsteihaug(fnhess,fngrad,fn,gradold,xold)
% description: solves the local trust-region problem 
% min m(p) = fk + gradfk'*p+0.5*p'*B*p 
% with CG-Steihaug algorithm and returns step change p and
% p'*B*p, where B is the Hessian at xold. Input parameters:
% fnhess: function hessian name
% fngrad: function grad name
% fn: function name
% xold: starting point

% global variables:
global parms;

% local variables:
% ii: iteration variable

deltacurr = parms.deltacurr;
cgeps = parms.cgeps; 
maxcgiter = parms.maxcgiter;
tau = [0 0]';
p = zeros(size(xold));
r = feval(fngrad,xold);
d = -r;
B = feval(fnhess,xold);
for ii = (1:maxcgiter);
  if (norm(r) <= cgeps);
    pBp = p'*B*p;
    return;
  end
  dBd = d'*B*d;
  if (dBd <= 0) % if so, find p+tau*d that minimizes m in trust region
    % we won't check for zero d'*d, but let's do safe quadratics
    a = d'*d;
    b = (p'*d)/a;
    c = (p'*p-deltacurr^2)/a;
    if (b >= 0)
      tau(1) = (-b - sqrt(b^2 - c));
    else
      tau(1) = (-b + sqrt(b^2 - c));
    end
    tau(2) = c/tau(1);
    % now find minimum value of m
    dBp = d'*B*p;
    val = dBp*tau + dBd/2*tau.^2;
    minval = min(val);
    taumin = tau(find(val == minval));
    p = p + taumin(1)*d;
    pBp = p'*B*p;
    return;
  else
    alpha = r'*r/dBd;
    pnew = p + alpha*d;
    if (norm(pnew)>=deltacurr) % if so, find p+tau*d, tau >0, on bdy of trust region
      % we won't check for zero d'*d, but let's do safe quadratics
      a = d'*d;
      b = (p'*d)/a;
      c = (p'*p-deltacurr^2)/a;
      if (b >= 0)
        tau(1) = (-b - sqrt(b^2 - c));
      else
        tau(1) = (-b + sqrt(b^2 - c));
      end
      tau(2) = c/tau(1);
      % one has to be positive
      taumin = max(tau);
      p = p + taumin(1)*d; 
      pBp = p'*B*p;
      return;
    else
      p = pnew;
      rnew = r+alpha*B*d;
      d = -rnew + (rnew'*rnew/(r'*r))*d;
      r = rnew;
    end
  end
end
pBp = p'*B*p;