function [p,pGN]=dogleglsq(F,J,delta)
%DOGLEGLSQ Perform a double dogleg step in the Levenberg-Marquardt method.
%
%[p,pGN]=dogleglsq(F,J,delta)
%F     - residual at current point.
%J     - Jacobian at current point.
%delta - current size of region of trust.
%p     - double dogleg search direction, |p|<=delta.
%pGN   - Gauss-Newton search direction.

% v1.0  99-11-09. Niclas Borlin, niclas@cs.umu.se.

debug=1;

% Calculate Gauss-Newton direction.
% Article formuation.
% pN=H\(-g)
% Gauss-Newton formulation.
% pGN=J\(-F)

% pGN=-inv(J'J)*J'*F
% Use singular value decomposition of J (will be reused below if G-N step
% too long).
[U,S,V]=svd(J,0);
UTF=U'*F;
pGN=-(V*(S\UTF));

if (norm(pGN)<=delta)
	% Newton direction is short enough. Accept it.
	if (debug), disp('Gauss-Newton'); end
	p=pGN;
	return;
end

% Calculate the Cauchy Point.
% lambdaStar=g'*g/(g'*H*g);
% CP=-lambdaStar*g;

SUTF=diag(S).*UTF;
S2UTF=diag(S).*SUTF;
gTg=SUTF'*SUTF;
gTHg=S2UTF'*S2UTF;
lambdaStar=gTg/gTHg;
CP=-lambdaStar*J'*F;

if (norm(CP)>delta)
	% Cauchy Point outside region. Use scaled gradient.
	if (debug), disp('CP'); end
	% p=-g/norm(g)*delta
	p=-J'*F/norm(SUTF)*delta;
	return;
end

% Calculate Nhat.
% gamma=norm(g)^4/((g'*H*g)*(g'*(H\g)));
gamma=gTg^2/(gTHg*UTF'*UTF);
eta=0.8*gamma+0.2;
Nhat=eta*pGN;

if (norm(Nhat)<=delta)
	% Nhat is inside. Use scaled Newton.
	if (debug), disp('Scaled Gauss-Newton'); end
	p=pGN/norm(pGN)*delta;
	return;
end

% Find point on CP-nHat line with norm delta.
A=sum((CP-Nhat).^2);
B=sum(2*CP.*(Nhat-CP));
C=sum(CP.^2)-delta^2;

lambda=(-B+sqrt(B^2-4*A*C))/(2*A);

if (debug), disp('On line'); end
p=CP+lambda*(Nhat-CP);
