function [p,pBp] = dogleg(fnhess,fngrad,fn,gradold,xold)
% usage: [p,pBp] = dogleg(fnhess,fngrad,fn,gradold,xold)
% description: solves the local trust-region problem 
% min m(p) = fk + gradfk'*p+0.5*p'*B*p 
% using the dogleg algorithm. Returns p = xnew-xold and
% pBp = p'*B*p, where B is Hessian matrix.
% Input parameters:
% fnhess: function hessian name
% fngrad: function grad name
% fn: function name
% xold: starting point
% delta: trust region radius

% global variables:
global parms;

% local variables:
% delta: current trust region radius
% B: current Hessian
% gBg: gradold'*B*gradold
% normg: norm(gradold)
% pU: steepest descent point
% pB: Newton point
% pQ: pB-pU
% normpU,normpQ: norm(p*)
% b,c: temporary coefs in a quadratic
% t1, t2: roots of quadratic
% taum1: tau - 1 of dogleg formula

% initialize
delta = parms.deltacurr;
% compute the steepest descent point
B = feval(fnhess,xold);
gBg = gradold'*B*gradold;
normg = norm(gradold);
if (gBg <= 0) % if negative curvature revert to steepest descent
  if (normg == 0)
    p = gradold;
  else
    p = -delta*gradold/normg; % note we don't chech for div by 0
  end
else
  pU = -gradold'*gradold/gBg*gradold;
  normpU = norm(pU);
  if (normpU >= delta) % if so, don't need pB here
    p = delta*pU/normpU;
  else % ok, do need Newton point
    pB = -B\gradold;
    if (norm(pB) <= delta) % newton point in trust region?
      p = pB;
    else % no
      pQ = pB - pU;
      pUpQ = pU'*pQ;
      if (pUpQ <= 0) % indicates negative curvature, revert to steep descent
        p = pU;
      else
        normpQ = norm(pQ);
        % we won't check for zero normpQ, but let's do safe quadratics
        b = pUpQ/normpQ^2;
        c = (normpU^2-delta^2)/normpQ^2;
        t1 = (-pUpQ -sign(pUpQ)*sqrt(b^2 -c));
        t2 = c/t1;
        taum1 =  min(1,max(t1,t2)); % we want the positive root, should be <= 1
        p = pU + taum1*pQ;
      end
    end
  end
end
pBp = p'*B*p;