function [x,y,out] = mwd1(f,options)
% Minimization of a continuous function f: R --> R union {Inf}
%
% Florian Jarre, March 2015.
%
% Simplest calling routine
%
%   x = mwd1(@f);
%
% or, with more specifications:
%
%   [x,fx,out] = mwd1(@f,options);
%
% Test Version with errors -- No guarantee of any kind is given!!!
%
% Algorithms: Some form of bisection and spline interpolation.
%             Return the lowest point that the algorithm stumbles about
%             (in contrast to the official Matlab routine)
%
%
% INPUT: 
%   Mandatory: A function handle f,
%   Optional:  The structure options as outlined below.
%
%   options.x0   - The minimizer is restricted to the interval [x0,x1]. 
%                  (If x1 < x0, the interval [x1,x0] is considered 
%                   instead.) (Default of x0 is -Inf)
%   options.x1   -            (Default of x1 is  Inf)
%                  If x0 or x1 are specified f will be evaluated only
%                  within the given bounds
%   options.xact - Reference point for the line search. 
%                  When x0 <= xact <= x1 the returned value shall be at 
%                  least as good as f(xact).
%
%   options.tol  - An approximate tolerance for the minimizer x:
%                  In the nonsmooth case there exists a local minimizer xm
%                  of f satisfying |xm - x| <= tol * int_length where 
%                  int_length is the length of a sub interval of [x0,x1] 
%                  generated by the algorithm. (Default tol = 1e-8)
%                  In the smooth case the above stopping criterion is
%                  based on an estimate - and is not guaranteed!
%                  WARNING: 
%                  Direct search for smooth minimization can never get more
%                  than about half the digits of full machine precision
%
%   options.smth - Binary variable indicating whehter f can be considered
%                  to be a smooth function. (If yes, spline interpolation
%                  is applied, if not plain golden mean search is used.)
%                  Default is 1
%
%
% OUTPUT:
%   x        -- some approximate minimizer
%   y        -- the associated function value
%   out.it   -- specifies the number of function evaluations needed.
%
%
% Subroutines used:
%           find_spline.m -- find a least squares spline through xx and ff
%           eval_spline.m -- evaluate the spline at a given point
%           min_spline.m  -- find the minimizer of the spline

done    = 0;    % We are not done yet


% COMPLETE OPTIONAL INPUT AND GENERATE STARTING POINT xact in (x0,x1):

if (nargin < 2)
   options.x0   = -Inf;
   options.x1   =  Inf;
   options.xact = 0.0;
   options.tol  = 1.0e-8;
   options.smth = 1;
end

% Set x0 and x1: 
if ~isfield(options,'x0') 
   options.x0 = -Inf; 
end

if ~isfield(options,'x1')
   options.x1 =  Inf;
end

x0 = options.x0;
x1 = options.x1;
if x1 < x0
   tmp = x1; x1 = x0; x0 = tmp; 
   warning('lower and upper bound were exchanged in line search');
end

iterations = 0;

if x0 == x1
   if x0 == -Inf || x0 == Inf
      error('bounds in line search are inconsistent');
   end
   options.xact = x0;
   xact         = x0;
   fact         = f(xact); f0 = fact; f1 = fact;
   iterations   = iterations + 1;
   done         = 1;
   warning('Trivial line search in interval of length zero');
end

% Set xact:
if ~isfield(options,'xact') 
   options.xact = x0;
end
xact = options.xact;
if xact <= x0 || xact >= x1 % Make sure xact is in the interior of [x0,x1]
   if x0  > -Inf
      if x1 < Inf
         xact = 0.5*(x0+x1);
      else
	     xact = x0 + 0.5*abs(x0) + 1;
      end
   else % Case x0 = -Inf and then
      if x1 < Inf
         xact = x1 - 0.5*abs(x1) - 1;
      else
         xact = 0;
      end
   end
end
if done == 0;
   fact = f(xact);
   iterations = iterations + 1; % Number of function evaluations
end
% Now, x0 < xact < x1  but x0 = -Inf of x1 = Inf is possible


% Set the tolerance
if ~isfield(options,'tol') 
   options.tol = 1.0e-8;
end
tol = min(0.1,max(1.0e-8,options.tol)); % Do not allow very high precision 
                                        % or very low precision

if ~isfield(options,'smth') % Assume smooth minimization
   options.smth = 1;
end

                                        
% Complete function values for x0, x1
if done == 0
   f0 = Inf;
   f1 = Inf;
end
if done == 0 && x0 > -Inf
   f0 = f(x0); iterations = iterations+1;
end
if done == 0 && x1 < Inf
   f1 = f(x1); iterations = iterations+1;
end




% GENERATE FINTE BOUNDS

dt = 1 + abs(xact)*1.0E-3; 
% (for large xact an increment by one may be too small)
if x1 < Inf
   dt = max(dt,x1-xact);
end
if x0 > -Inf
   dt = max(dt,xact-x0);
end
dtsave = dt;


% Make one of the bounds finite
if x0 == -Inf && x1 == Inf % Here, it must be ``done = 0''
   xtmp = xact+dt;
   ftmp = f(xtmp); iterations = iterations+1;
   if ftmp < fact
      x0 = xact;
      f0 = fact;
      xact = xtmp;
      fact = ftmp;
   else
      x1 = xtmp;
      f1 = ftmp;
   end
end


% Generate a finite upper bound
if x1 == Inf && f0 >= fact % Here, it must be ``done = 0''
   xtmp = xact + dt;
   ftmp = f(xtmp); iterations = iterations+1;
   itcount = 0;
   while itcount < 15 && ftmp < fact % Line search up to length 10^15
      x0 = xact;
      f0 = fact;
      xact = xtmp;
      fact = ftmp;
      dt = dt*10;
      xtmp = xact + dt;
      ftmp = f(xtmp); iterations = iterations+1; itcount = itcount+1;
   end
   if itcount >= 15
      done = 1;
      if ftmp < fact
         xact = xtmp;
         fact = ftmp;
      end
      out.it = iterations;
      warning('line search may be unbounded (x to Inf)');
   else % itcount < 15 means ftmp >= fact
      x1 = xtmp;
      f1 = ftmp;
   end
end

if x1 == Inf && f0 < fact % Here, it must be ``done = 0''
   x1 = xact;
   f1 = fact;
   xact = 0.5*(xact+x0);
   fact = f(xact); iterations = iterations +1;
end


% Generate a finite lower bound
dt = dtsave;
if x0 == -Inf && f1 >= fact && done == 0
   xtmp = xact - dt;
   ftmp = f(xtmp); iterations = iterations+1;
   itcount = 0;
   while itcount < 15 && ftmp < fact % line search up to length 10^15
      x1 = xact;
      f1 = fact;
      xact = xtmp;
      fact = ftmp;
      dt = dt*10;
      xtmp = xact - dt;
      ftmp = f(xtmp); iterations = iterations+1; itcount = itcount+1;
   end
   if itcount >= 15
      done = 1;
      if ftmp < fact
         xact = xtmp;
         fact = ftmp;
      end
      out.it = iterations;
      warning('line search may be unbounded (x to -Inf)');
   else % itcount < 15 means ftmp >= fact
      x0 = xtmp;
      f0 = ftmp;
   end
end

if x0 == -Inf && f1 < fact && done == 0
   x0 = xact;
   f0 = fact;
   xact = 0.5*(xact+x0);
   fact = f(xact); iterations = iterations +1;
end
int_length = x1-x0; % Length of the interval containing the minimizer
int_length = max(int_length,10*eps*(abs(x0)+abs(x1))/tol);


% Eliminate Inf-values of f
if min([f0,fact,f1]) == Inf
   error('No finite value of f found in line search')
end
itcount = 0;
if fact == Inf
   if f1 < f0 % look for minimizer near x1
      while fact == Inf && itcount < 15
         itcount = itcount+1;
         x0 = xact; 
         f0 = fact;
         xact = 0.1*xact+0.9*x1;
         fact = f(xact); iterations = iterations+1;
      end
      if fact == Inf
         error('Failure in line search, only finite value may be x1')
      end 
   else % look for minimizer near x0
      while fact == Inf && itcount < 15
         itcount = itcount+1;
         x1 = xact; 
         f1 = fact;
         xact = 0.1*xact+0.9*x0;
         fact = f(xact); iterations = iterations+1;
      end
      if fact == Inf
         error('Failure in line search, only finite value may be x0')
      end 
   end
end % Now, fact is finite
if f1 == Inf
   while f1 == Inf && itcount < 30
      x1old = x1;
      x1 = 0.5*(xact+x1);
      f1 = f(x1); iterations = iterations+1;
   end
   if f1 == Inf
      error('f may be discontinuous at xact')
   else
      if f1 < min(f0,fact)
         x0 = xact;
         f0 = fact;
         xact = x1;
         fact = f1;
         x1 = x1old;
         f1 = Inf;
      end
   end
end
if f0 == Inf
   while f0 == Inf && itcount < 30
      x0old = x0;
      x0 = 0.5*(xact+x0);
      f0 = f(x0); iterations = iterations+1;
   end
   if f0 == Inf
      error('f may be discontinuous at xact')
   else
      if f0 < min(f1,fact)
         x1 = xact;
         f1 = fact;
         xact = x0;
         fact = f0;
         x0 = x0old;
         f0 = Inf;
      end
   end
end      
       





% MAKE SURE f0 AND f1 ARE AT LEAST AS LARGE AS fact 

itcountmax = 9+round(-log(tol)/log(10)); % higher precision near end points
% Number of iterations to identify a minimizer near the boundary

if fact > min(f0,f1) && done == 0
   itcount = 0;
   if f0 < f1
      while fact >= f0 && itcount < itcountmax
         x1 = xact; f1 = fact; itcount = itcount+1;
         xact = 0.9*x0+0.1*x1; fact  = f(xact); 
      end
   else
      while fact >= f1 && itcount < itcountmax
         x0 = xact; f0 = fact; itcount = itcount+1;
         xact = 0.1*x0+0.9*x1; fact  = f(xact); 
      end
   end
   iterations = iterations + itcount;
   if itcount >= itcountmax
      done = 1;
      if f0 < min(f1,fact)
         xact = x0;
         fact = f0;
      end
      if f1 < min(f0,fact)
         xact = x1;
         fact = f1;
      end
      out.it = iterations;
   end
end
% Now, either ``x1-x0 <= tol*int_length'' or ``fact < min(f0,f1)''
if xact <= x0 || x1 <= xact || f0 < fact || f1 < fact
   if done == 0
      error( ' programming error in line search 1' );
   end
end






% NOW, THE ACTUAL LINE SEARCH APPROXIMATING A MINIMIZER IN [x0,x1]

itcount = 0;                      % iteration counter
gm6 = 2/(sqrt(5)+1);              % Golden mean ratio, this is about 0.6
xfval = [x0,xact,x1;f0,fact,f1];  % Record all values of the search
iact = 2;                         % Index of xact in xfval

    
          
   
while done == 0 % *** MAIN LOOP ***
   spline_int = 0;
   itcount = itcount+1;

   % One golden mean search step
   if x1 - xact > xact - x0
      xa = x1 + gm6*(xact-x1); 
      fa = f(xa); iterations  = iterations + 1;  
      xfval = [xfval(:,1:iact),[xa;fa],xfval(:,iact+1:end)];
      if fa <= fact
         x0 = xact; xact = xa; fact = fa; iact = iact+1;
      else
         x1 = xa; 
      end      
   else
      xa = x0 + gm6*(xact-x0); 
      fa = f(xa); iterations  = iterations + 1;
      xfval = [xfval(:,1:iact-1),[xa;fa],xfval(:,iact:end)];
      if fa <= fact
         x1 = xact; xact = xa; fact = fa;
      else
         x0 = xa; iact = iact+1;
      end
   end % of golden mean step     
   if x1 - x0 <= tol*int_length
       done = 1;
   end
      
   
   % Test whether to use spline interpolation
   n = length(xfval);
   if n >=5 && options.smth == 1 && done == 0
      % Check whether the spline would have predicted fact correctly
      % Find points close to xact on both sides (2 <= iact <= end-1)
      if n == 5
         indspl = [1:iact-1,iact+1:5];
      else
         i_set = 0; % indspl not yet set
         if iact <= 2
            if iact <= 1
                error( 'programming error in line search')
            end
            indspl = [1,3,4,5,6]; 
            i_set = 1; % do not change indspl any more 
         end
         n = length(xfval);
         if iact >= n-1
            if iact >= n
                error( 'programming error in line search')
            end
            indspl = [n-5,n-4,n-3,n-2,n];
            i_set = 1; % do not change indspl any more
         end
         if i_set == 0 % now, 3 <= iact <= n-2
            indspl0 = [iact-2,iact-1,iact+1,iact+2]; 
            tmp     = [-Inf,xfval(1,:),Inf]; % Note: tmp(iact+1)=xact
            if tmp(iact+1)-tmp(iact-2) < tmp(iact+4)-tmp(iact+1)
               indspl = [iact-3,indspl0]; 
            else
               indspl = [indspl0,iact+3]; 
            end
         end
      end % Index for spline is set
      xx = xfval(1,indspl);
      ff = xfval(2,indspl);
      ss = find_spline(xx,ff); % Obiges koennte man direkt einsetzen!
      [sact,i] = eval_spline(xact,ss,xx);
      ip1 = min(1+1,length(xx)-1);
      if abs(sact-fact) < 0.1*(abs(ss(3,i))+abs(ss(3,ip1)))*...
                          (xact-xx(i))*(xx(i+1)-xact)
          spline_int = 1; % Estimate of second derivative is 80% correct
      end
   end
      
      
   % Possible spline acceleration (several steps)
   while spline_int == 1 && itcount < 100 && done == 0
      itcount = itcount + 1;
         
      % Recompute the spline including xact
      indspl = iact-2:iact+2; 
      n = length(xfval);
      if iact <= 1 || iact >= n
         error( 'programming error in line search')
      end
      if iact == 2
         indspl = 1:5;
      end
      if iact == n-1
         indspl = n-4:n;
      end
      xx = xfval(1,indspl);
      ff = xfval(2,indspl);
      ss = find_spline(xx,ff); % Obiges koennte man direkt einsetzen!
          
      [t,tval,i] = min_spline(ss,xx);
      if t <= x0 || t >= x1 % x0 = xfval(1,iact-1),  x1 = xfval(1,iact+1)
         spline_int = 0;
      else %%% Spline interpolation may be o.k. %%%
             
         ixf = i+indspl(1)-1; % t in [xfval(1,ixf),xfval(1,ixf+1)]
         if t < xx(i) || t > xx(i+1) || t<xfval(1,ixf) || t>xfval(1,ixf+1)
            error(' programming error in spline-line search ')
         end
         shift_t = 0.1*min(tol*int_length, xx(i+1)-xx(i));
         % less than tol*int_length even in the presence of rounding errors 
         if t < xx(i)+shift_t 
            t = xx(i)+shift_t;
         end
         if t > xx(i+1)-shift_t
            t = xx(i+1)-shift_t;
         end
         ft = f(t); iterations = iterations+1;
         epp = abs(tval-ft); % The approximation error at t
         ip1 = min(1+1,length(xx)-1);
         if epp > 0.2*(abs(ss(3,i))+abs(ss(3,ip1)))*(t-xx(i))*(xx(i+1)-t)
             spline_int = 0; % Prediction not so accurate
             %disp('return to golden mean search')
         end
         
         if epp < 100*eps*(abs(ft)+1)
            done = 1; % Prediction is close to machine precision
         end
         epp = 2*epp/((t-xx(i))*(xx(i+1)-t)); % Second der. of the error
         mss = ((xx(i+1)-t)*ss(3,i)+(t-xx(i))*ss(3,ip1))/(xx(i+1)-xx(i));
         if mss > epp
            tmp = max(0.1*(xx(i+1)-xx(i)), abs(0.5*(xx(i+1)+xx(i))-t) );
            if tmp*epp/(mss-epp) < int_length*tol
               done = 1; % prediction of error in t is small
            end
         end
         xfval = [xfval(:,1:ixf),[t;ft],xfval(:,ixf+1:end)];
         if t < xact
            if ft > fact
               iact = iact+1; % xact remains same but iact increases by 1
            end
         else
            if ft < fact
               iact = iact + 1; % xact changes and also iact increases by 1
            end
         end
            
         x0   = xfval(1,iact-1);
         xact = xfval(1,iact  ); 
         fact = xfval(2,iact  );
         x1   = xfval(1,iact+1);
         if x1-x0 <= tol*int_length
            done = 1;
         end
      end %%% Case where spline interpolation may be o.k. %%%
   end   
   if x1-x0 <= tol*int_length || itcount >= 100
      done = 1;
   end 
end % *** OF MAIN LOOP ***
x = xact; y = fact; out.it = iterations;
end






function s = find_spline(x,f)
% Find the least squares spline interpolant through the points x(i), f(i)
% assuming that there are at least 4 interpolation points.
% On each interval [x(j),x(j+1)] (1<=j<=n-1) the spline is represented as
% s(x) = s(1,j) + s(2,j)*(x-x(j)) + s(3,j)*(x-x(j))^2 + s(4,j)*(x-x(j))^3

% Initialize
[n,m] = size(x);
if n < m % make x a column vector
   x = x'; tmp = n; n = m; m = tmp;
end
if m > 1 || m == 0 || n < 4
  error('input x for spline interpolation is inconsistent');
end

[nn,mm] = size(f);
if nn < mm % make f a column vector
   f = f'; tmp = nn; nn = mm; mm = tmp;
end
if mm > 1 || mm == 0
  error('input f for spline interpolation is inconsistent');
end

if n ~= nn
  error('input x,f for spline interpolation is inconsistent');
end

dx = x(2:n)-x(1:n-1); % vector of increments of x
if min(dx) <= 0
error('vector of spline base points is assumed to be in increasing order');
end

s  = zeros(12,n); % Three splines with n-1 cubic parts each.
% The last column is a dummy to allow assigning f as the first row.
% The first spline interpolates f,
% the second and third interpolate the zero function.
% rows 1 to 4  for the spline interpolating f
% rows 5 to 8  for the spline interpolating zero; nonzero linear first term
% rows 9 to 12 for the spline interpolating zero; nonzero quadr. first term
t1 = [1,5, 9]; % indices for the constant terms
t2 = [2,6,10]; % indices for the linear terms
t3 = [3,7,11]; % indices for the quadratic terms
t4 = [4,8,12]; % indices for the cubic terms

s(1,:) = f; % the constant part of s(1:4,:) is given by f
            % (constant parts of the zero splines are zero)


% Set up the three splines; first the splines on [x(1),x(2)]
s(2,1)  = (f(2)-f(1))/dx(1); % first spline, linear on [x(1),x(2)]
s(6,1)  = 1;                 % second spline, first linear term is 1
s(8,1)  = -1/dx(1)^2;        % interoplate to zero at x(2)
s(11,1) = 1;                 % third spline, first quadratic term is 1
s(12,1) = -1/dx(1);          % interoplate to zero at x(2)

% then the interpolating splines on the remaining subintervals
for i = 2:n-1
   s(t2,i) = s(t2,i-1)+dx(i-1)*(2*s(t3,i-1)+3*dx(i-1)*s(t4,i-1));
   s(t3,i) = s(t3,i-1)+3*dx(i-1)*s(t4,i-1);
   s(t4,i) = (s(t1,i+1)-s(t1,i)-dx(i)*(s(t2,i)+dx(i)*s(t3,i)))/dx(i)^3;
end
s = s(:,1:n-1); % now remove the last column

% The norm is given by || D * \Delta * s(4,:)' ||_2 where, formally,
% D      = Diag(((dx(2:n-1)+dx(1:n-2)).^-.5); and
% \Delta = [-eye(n-2),zeros(n-2,1)]+[zeros(n-2,1),eye(n-2)];
% i.e. a weighted sum of squared jumps of the third derivatives

d  = (dx(2:n-1)+dx(1:n-2)).^-.5; % weights
Ds = (kron(ones(3,1),d').*(s(t4,1:n-2)-s(t4,2:n-1)))';
% Ds contains the weighted jumps in the cubic terms of the splines

% orthogonalize the third with respect to the second spline
alpha     = Ds(:,3)'*Ds(:,2)/(Ds(:,2)'*Ds(:,2));
s(9:12,:) = s(9:12,:)-alpha*s(5:8,:); % updating s(10:12,:) would suffice
Ds(:,3)   = Ds(:,3)  -alpha*Ds(:,2);

% adjust first with the second spline
alpha     = Ds(:,1)'*Ds(:,2)/(Ds(:,2)'*Ds(:,2));
s(1:4,:)  = s(1:4,:)-alpha*s(5:8,:); % updating s(2:4,:) would suffice
Ds(:,1)   = Ds(:,1) -alpha*Ds(:,2);

% adjust first with the third spline
alpha     = Ds(:,1)'*Ds(:,3)/(Ds(:,3)'*Ds(:,3));
s(1:4,:)  = s(1:4,:)-alpha*s(9:12,:); % updating s(2:4,:) would suffice
%Ds(:,1)   = Ds(:,1) -alpha*Ds(:,3); % not needed

% to reduce rounding errors redo the final spline
s = s(1:4,:);
for i = 2:n-1
   s(2,i) = s(2,i-1)+dx(i-1)*(2*s(3,i-1)+3*dx(i-1)*s(4,i-1));
   s(3,i) = s(3,i-1)+3*dx(i-1)*s(4,i-1);
   s(4,i) = (f(i+1)-f(i)-dx(i)*(s(2,i)+dx(i)*s(3,i)))/dx(i)^3;
end
end






function [y,i] = eval_spline(xact,s,x)
% Evaluate the spline given by s and the partition x at the point xact
% Also return the segment in which xact is located
% Assume x in increasing order, length(x) = n and s is 1:4 by 1:n-1

n = length(x);

if xact < x(1) || xact > x(n)
    error(' point out of range in spline evaluation ');
end
i = 1;
while xact > x(i+1)
   i = i+1;
end

t = xact-x(i);
y = s(1,i)+t*(s(2,i)+t*(s(3,i)+t*(s(4,i))));
end







function [t,y,imin] = min_spline(s,x)
% Find the minimum of the spline given by s in the interval [x(1),x(end)]
% The minimizer t with value y in the interval [x(imin),x(imin+1)]
% Assume x in increasing order, length(x) = n and s is 1:4 by 1:n-1

n = length(x);

% Test support points first
[y,imin] = min(s(1,:)); t=x(imin);
dx = x(n)-x(n-1);
y_last = s(1,n-1)+dx*(s(2,n-1)+dx*(s(3,n-1)+dx*(s(4,n-1))));
if y_last < y
   y = y_last; t = x(n); imin = n-1; % Not imin = n, so x(imin+1) exists
end

for i = 1:n-1
   v = s(:,i); % just for convenience
   if v(4) ~= 0
      vv = v(1:3)/(3*v(4));
      if vv(3)^2 >= vv(2)
         root = sqrt(vv(3)^2-vv(2));
         t1 = -vv(3)-root;
         if t1 > 0 && t1 < x(i+1)-x(i)
            y1 = s(1,i)+t1*(s(2,i)+t1*(s(3,i)+t1*(s(4,i))));
            if y1 < y
               y = y1; t = x(i)+t1; imin = i;
            end
         end
         t1 = -vv(3)+root;
         if t1 > 0 && t1 < x(i+1)-x(i)
            y1 = s(1,i)+t1*(s(2,i)+t1*(s(3,i)+t1*(s(4,i))));
            if y1 < y
               y = y1; t = x(i)+t1; imin = i;
            end
         end
      end
   else
      if v(3) ~= 0
         t1 = -0.5*v(2)/v(3);
         if t1 > 0 && t1 < x(i+1)-x(i)
            y1 = s(1,i)+t1*(s(2,i)+t1*(s(3,i)+t1*(s(4,i))));
            if y1 < y
               y = y1; t = x(i)+t1; imin = i;
            end
         end
      end
   end
end
end


