function Par = TR(XY,ParIni,DeltaIni)

%--------------------------------------------------------------------------
%  
%     Circle fit by Trust Region 
%        in the full (a,b,R) parameter space
%
%     Input:  XY(n,2) is the array of coordinates of n points x(i)=XY(i,1), y(i)=XY(i,2)
%             ParIni = [a b R] is the initial guess
%             DeltaIni  is the initial size of the trust region
%               (this is optional; if it is missing, TR sets it to 1)
%
%     Output: Par = [a b R] is the fitting circle:
%                           center (a,b) and radius R
%             Iter is the number of (main) iterations
%
%--------------------------------------------------------------------------

if (nargin < 3), DeltaIni = 1; end;  % if Delta(initial) is not supplied, set it to one

Delta = DeltaIni;     % starting with the given initial guess 

epsilon=0.000001;    % tolerance (small threshold)

IterMAX = 50;      % maximal number of (main) iterations; usually 5-10 suffice 

c1=0.1; c2=0.25; c3=0.75;  % technical constants for updating Delta

nPar = 3; % number of parameters in the model

Par = ParIni;     % starting with the given initial guess

[J,g,F] = CurrentIteration(Par,XY);    % compute objective function and its derivatives

g0 = [g; zeros(nPar,1)];   % "extended" version of the g-vector

for iter=1:IterMAX         %  main loop, each run is one (main) iteration
    
    while (1)         %  secondary loop - adjusting Lambda (no limit on cycles)
    
        lambda = 0.0001;     %  initial value for Lambda
        h = [J; sqrt(lambda)*eye(nPar)]\g0;   % try the Gauss-Newton step
        if (norm(h) > (1+c1)*Delta)   % check if the Gauss-Newton step is outside trust region
            lambda_min = 0;   
            lambda_max = 2;
            while (2)   % estimate lambda_max (upper bound for Lambda)
                lambda_max = lambda_max*lambda_max;
                h = [J; sqrt(lambda_max)*eye(nPar)]\g0;
                if (norm(h) < Delta), break, end
            end  %  while (2)   now lambda_max can be used
        
            lambda = 0.01;
        
            while (3)  % solving the subproblem (find Lambda given Delta)
                [Q R] = qr([J; sqrt(lambda)*eye(nPar)],0);  
                Rinv = inv(R);
                h = Rinv*(Q'*g0);    %  potential step
                phi = norm(h) - Delta;  %  need phi=0, ideally
                if (abs(phi)<c1*Delta), break, end;  % the subproblem solved
                Rinvh = Rinv'*h;
                phiprime = -norm(Rinvh)^2/norm(h);  % the derivative of phi(lambda)
                lambda_max_new = lambda;
                lambda_min_new = lambda - phi/phiprime;
                lambda = lambda - (phi+Delta)/Delta*(phi/phiprime);  % new lambda candidate
                lambda = max(lambda,lambda_min);
                lambda = min(lambda,lambda_max);
                lambda_min = max(lambda_min,lambda_min_new);
                if (phi<0) lambda_max = lambda_max_new; end;
            end  %  while (3)
        end  %  if

%                  step h is now computed, check if it is acceptable

        progress = norm(h)/(Par(3)+epsilon);
        if (progress < epsilon) break; end

        ParTemp = Par - h';
        [JTemp,gTemp,FTemp] = CurrentIteration(ParTemp,XY);
        ratio = (F - FTemp)/((norm(J*h))^2 + 2*lambda*(norm(h))^2);
        if (ratio < c2 || ParTemp(3) < 0) Delta = min(Delta,norm(h))/2; end;
%                    (little or no improvement, reduce the trust region)
        if (ratio > c3 && ParTemp(3) > 0 && Delta < 2*norm(h)) Delta = 2*Delta; end;  
%                    (big improvement, enlarge the trust region)
        if (ratio > 0. && ParTemp(3) > 0) break; end;   
%                    (the step is acceptable, move on)
    end  %  while (1), the end of the secondary loop
 
    if (progress < epsilon) break; end   %  stopping rule

    Par = ParTemp;  J = JTemp;  g = gTemp;  F = FTemp;  g0 = [g; zeros(nPar,1)];
%                (update the iteration)

end   % the end of the main loop (over iterations)
end   %   TR

%================ function CurrentIteration ================

function [J,g,F] = CurrentIteration(Par,XY)

%    computes F and its derivatives at the current point Par

Dx = XY(:,1) - Par(1);
Dy = XY(:,2) - Par(2);
D = sqrt(Dx.*Dx + Dy.*Dy);
J = [-Dx./D, -Dy./D,  -ones(size(XY,1),1)];
g = D - Par(3);
F = norm(g)^2;

end  %  CurrentIteration