% dogleg.m -- Trust Region/Dogleg script for minimization
%
% Script applies Newton-based TR method to a given function, 
% using workspace variables described below:
%   fname  - string containing name of function 
%   dfname - string containing name of gradient function
%   hfname - string containing name of Hessian function
%   x0     - column vector containing starting point
% Stopping test parameters are defined near the top, and can be
% changed without harming what remains.  These are
%   eta1   - relative error among components of the input vector x
%   eta2   - tolerance for the gradient being nearly zero
%   Kmax   - maximum number of iterations

% (c) 2001, Philip D. Loewen
% 23 Feb 01  --  Original
% 10 Mar 01  --  Modified Trust Region Radius Update

eta1 = 100*sqrt(eps); % input-point-based termination tolerance.
eta2 = 1.e4*eps;   % gradient-based termination tolerance.
Kmax = 30;         % Students were asked to use 25
RRT  = 1/5;        % Reduction Ratio Threshhold ("eta" in notes).

Delta = 0.1;       % Initial value for trust region radius.
Dmax = 100;        % Absolute maximum trust region radius.

Zeps = eps^(3/4);  % Prevents division by 0 in stopping tests.

% Set up table titles
disp([' ']);
disp(['Newton/Trust Region minimization of function "',fname,'",']);
disp(['using gradient "',dfname,'", Hessian "',hfname,'":']);

xstring = '';
for jj=1:length(x0),
  xstring = [xstring,'x_k(',int2str(jj),')        '];
end;
disp([' ']);
disp(['  k     ',xstring,'f(x_k)        Delta   |dx|/Delta   RR', ...
      '     e_1       e_2']);

% Count flops for added interest
fsf = flops;

% Set current point, function, and gradient.
xc = x0;
fc = eval([ fname,'(xc)']);
gc = eval([dfname,'(xc)']);

% Build output line
disp(['  0', sprintf('  %12.4e',[xc',fc]),...
      sprintf('  %10.2e',Delta)]);

for k=1:Kmax,

  % Evaluate current Hessian and Newton point.
  Hc = eval([hfname,'(xc)']);
  pN = - Hc\gc';

  predredN = -(gc*pN + 0.5*pN'*Hc*pN); % Model reduction for Newton Pt

  gHg = gc*Hc*gc';
  
  seg = 0;
  if predredN>0,
    % Good situation: Newton point reduces the model function.
    pNlen = (pN'*pN)^0.5;
    if pNlen <= Delta, %Newton point in trust region: use it!
      seg = 3;
      xt = xc + pN;
    else    
      % Find Unconstrained Steepest Descent Minimizer for Model Function.
      pSD = -(gc*gc')/gHg*gc';
      pSDlen = (pSD'*pSD)^0.5;
      if pSDlen>=Delta,
        seg = 1;
        % Both Newton and SD pts lie outside, use Cauchy pt on boundary.
        xt = xc - Delta*gc'/(gc*gc')^0.5;
      else
        seg = 2;
        % SD pt inside, good Newton pt outside.  Find boundary point
        % of trust region on line joining them.
        pN_pSD = pN-pSD;
        aa = pN_pSD'*pN_pSD;
        bb = 2*pN_pSD'*pSD;
        cc = pSD'*pSD - Delta^2;
        t  = (-bb+(bb^2-4*aa*cc)^0.5)/2/aa;
        xt = xc + pSD + t*pN_pSD;
      end
    end
  else  % Bad situation: Newton Pt *increases* model function!
    if gHg>5*eps,
      % 1D function is convex enough:  
      % Find Unconstrained Steepest Descent Minimizer for Model Function.
      pSD = -(gc*gc')/gHg*gc';
      pSDlen = (pSD'*pSD)^0.5;
      if pSDlen <= Delta,
        xt = xc + pSD;
      else
        xt = xc + Delta*pSD/pSDlen;
      end;
    else
      % 1D slice of model function in gradient direction is concave:
      % SD minimizer lies on TR boundary, in direction of -g'
      xt = xc - Delta*gc'/(gc*gc')^0.5;
    end;
  end
  
  % Trust-Region subproblem has been "solved" above:
  % solution is called xt for "x-trial".
  % Find function and gradient values.
  ft = eval([fname,'(xt)']);
  gt = eval([dfname,'(xt)']); 
  
  dx = xt - xc;   % Record change in x-vector.
  df = ft - fc;   % Record change in f-values (should be negative).

  actred = -df;   % Reduction observed on objective function.
  predred= -gc*dx - 0.5*dx'*Hc*dx;  % Reduction predicted by model.
  RR     = actred/predred;   % Reduction Ratio.

  %  disp(' ');
  %  disp([sprintf('%3d  actred = %10.2e, predred = %10.2e, eig(Hc) = ',...
  %                                                 [k,actred,predred]),...
  %        sprintf(' %10.2e',eig(Hc)')] );
    
  % Update the current point
  if RR>=RRT,
    xn = xt;    % Good reduction, accept trial point.
    fn = ft;
  else
    xn = xc;    % Poor reduction, retain current point.
    fn = fc;
  end;
  gn = eval([dfname,'(xn)']);
  
  % Evaluate stopping criteria.
  e1 = max(abs(pN') ./ max([abs(xc');Zeps*ones(size(xc'))])); % Rel length of Newton step
  e2 = max( (abs(gn') .* abs(xn)) / max([abs(fn),Zeps]) );    % Max comp of grad(f)

  % Build and display step report
  disp([sprintf('%3d',k), sprintf('  %12.4e',[xn',fn]),...
        sprintf('  %10.2e',Delta),...
        sprintf('  %6.4f',norm(dx)/Delta),sprintf('  %7.3f',RR),...
        sprintf('  %8.1e',e1),...
        sprintf('  %8.1e',e2)]);

  xc = xn;                     % Update the x-vector
  fc = fn;                     % Update the function value
  gc = gn;                     % Update the gradient
  
  % Update the trust region radius:
  if RR<0.25, Delta = 0.25*norm(dx); end;   %%% Focus here.
  if RR>0.75 & norm(dx,2)>((1-eps)*Delta),
    Delta = min([2*Delta;Dmax]);
  end;
  
  % Act on stopping criteria.
  sr = zeros(2,1);
  if (e1<eta1 & predredN>0), sr(1)=1; end;
  if e2<eta2, sr(2)=1; end;
  if sr(1)|sr(2), break; end;
    
end;    % Finished main iteration.

totflops = flops-fsf;

% Print results.
disp([' ']);
disp(['Best point:  x'' =',sprintf(' %22.16e',xc),'.']);
disp(['Best value:  ',fname,'(x) =',sprintf(' %22.16e',fc),'.']);
disp(['Step count:  ',int2str(k),' Trust-Region passes.']);

ol = '';
if sr(1), 
  ol = 'e1 < eta1'; 
  if sr(2),
    ol = [ol,' and '];
  end;
end;
if sr(2),
  ol = [ol,'e2 < eta2'];
end; 
if k==Kmax,
   ol = 'iteration limit reached';
end;
disp(['Stopped by:  ',ol,'.']);

disp(['Flop count:  ',int2str(totflops),' total--average ',int2str(totflops/k),' per step.']);
disp(' ');

