function [x,fval,exitflag,output] = Anneal(funfcn,x,options,varargin)
%ANNEAL Simulated Annealing using a modified Nelder-Mead algorithm
%
%   VERSION WITH ANIMATION
%
%   X = ANNEAL(FUN,X0) returns a vector X that is a local
%   minimizer of the function that is described in FUN (usually an M-file: 
%   FUN.M) near the starting vector X0.  FUN should return a scalar function 
%   value when called with feval: F=feval(FUN,X).  See below for more options 
%   for FUN.
%
%   X = ANNEAL(FUN,X0,OPTIONS)  minimizes with the default optimization
%   parameters replaced by values in the structure OPTIONS, created
%   with the ANNEALSET function.  See ANNEALSET for details.  ANNEAL uses
%   these options: Display, TolX, TolFun, MaxFunEvals, and MaxIter. 
%
%   X = ANNEAL(FUN,X0,OPTIONS,P1,P2,...) provides for additional arguments 
%   which are passed to the objective function, F=feval(FUN,X,P1,P2,...).
%   Pass an empty matrix for OPTIONS to use the default values.
%   (Use OPTIONS = [] as a place holder if no options are set.)
%
%   [X,FVAL] = ANNEAL(...) returns a string returned as the second
%   argument of the objective function at the minimizer.
% 
%   [X,FVAL,EXITFLAG] = ANNEAL(...) returns a string EXITFLAG that 
%   describes the exit condition of ANNEAL.  %
%   If EXITFLAG is:
%     1 then ANNEAL converged with a solution X.
%     0 then the maximum number of iterations was reached.
%   
%   [X,FVAL,EXITFLAG,OUTPUT] = ANNEAL(...) returns a structure
%   OUTPUT with the number of iterations taken in OUTPUT.iterations
%   and the string returned by the minimized in OUTPUT.string;
%
%   The argument FUN can be an inline function:
%      f = inline('norm(x)');
%      x = Anneal(f,[1;2;3]);
%
%   ANNEAL uses the Nelder-Mead simplex (direct search) method modified
%   to accept improper transformations of the simplex with a probability
%   that decreases with temperature.
%
%   SIMPLEX SEARCH ALGORITHM operates as follows:
%     1. Initial guess forms first vertex of simplex
%     2. The other n verticies (n+1 total) are formed by adding 
%        USUAL_DELTA or ZERO_TERM_DELTA to each of the n coordinates
%     3. Cost function is evaluated at each vertex.
%   LOOP:
%     4. Noise added to each evaluation (T*log(rand)) where T is temperature.
%     5. Vertices are sorted by noisy cost (different noise each iteration)
%     6. N+1st vertex is REFLECTed across plane of other N.
%     7. If reflected value is less than 1st value, reflection is EXPANDed.
%        (expand = reflect farther) Note: (value = noisy value) throughout
%        A. If expanded value less than reflected value, expansion accepted
%        B. If expanded value not less than reflected, reflection accepted
%     8. If reflected value less than Nth value, but not less than 1st
%        reflection accepted
%     9. If reflected value between Nth and N+1st (reflected from) value
%        perform an OUTSIDE CONTRACTION (don't reflect as far, still across)
%        A. If contracted value less than refected value accept contraction.
%        B. Otherwise SHRINK.  (NRC variation not implemented OUTSIDE SHRINK). 
%           (Outside shrink = accept outside contraction then shrink).
%           (See 11. for shrink).
%     10. If reflected value greater than N+1st (reflected from) value
%        perform an INSIDE CONTRACTION
%        A. If contracted value less than N+1st value, accecpt contraction
%        B. Otherwise SHRINK.  (Shrink = Inside Shrink)
%     11. If Shrink: Shrink distance between 1st vertex and each other vertex
%        by some factor.
%    END LOOP: tolx and tolf checked
%
%   See also FMINSEARCH, ANNEALSET, ANNEALGET.


%   Adapted from the algorithm given in _Numerical Recipes in C_, page 452-454
%   And from FMINSEARCH
%   Copyright (c) 1984-98 by The MathWorks, Inc.
%   $Revision: 1.4 $  $Date: 2005/03/11 18:57:59 $
%
%   Reference: Jeffrey C. Lagarias, James A. Reeds, Margaret H. Wright,
%   Paul E. Wright, "Convergence Properties of the Nelder-Mead Simplex
%   Algorithm in Low Dimensions", May 1, 1997.  To appear in the SIAM
%   Journal of Optimization.
%   
%   Reference: William H. Press, William T. Vetterling, Saul A. Teukolsky
%   Brian P. Flannery, _Numerical Recipes in C_, Cambridge University Press,
%   1992

%   ADDING/CHANGING OPTIONS:
%     (1) Change 2nd paragraph of help comment 
%
%   TO DO:
%   (1) fix comment, what happens when no convergence?
%    > If EXITFLAG is:
%    > 0 then the maximum number of iterations was reached.

clk0 = clock;

defaultopt = AnnealSet('display','best',...
   'maxiter', '15*numberOfVariables', ...
   'maxfunevals', '15*numberOfVariables', ...
   'TolX',1e-4,'TolFun',1e-4, ...
   'bestsave', 'off', 'schedule', 'regular', 'tinitial', 1, ...
   'tfactor', .9, 'numsteps', 60, 'qfactor', 15, 'restart', 15);

% If just 'defaults' passed in, return the default options in X
if nargin==1 & nargout <= 1 & isequal(funfcn,'defaults')
   x = defaultopt;
   return
end

if nargin<3, options = []; end

n = prod(size(x));
numberOfVariables = n;

options = AnnealSet(defaultopt,options);
printtype = AnnealGet(options,'display');
tolx = AnnealGet(options,'tolx');
tolf = AnnealGet(options,'tolfun');
maxfun = AnnealGet(options,'maxfunevals');
maxiter = AnnealGet(options,'maxiter');
fname = AnnealGet(options,'bestsave');
schedule = AnnealGet(options,'schedule');
T = AnnealGet(options,'tinitial');
Tfactor = AnnealGet(options,'tfactor');
numsteps = AnnealGet(options,'numsteps');
qfactor = AnnealGet(options,'qfactor');
restart = AnnealGet(options,'restart');

% Cornell parameters
%   Tfactor = .9;
%   numsteps = 350;  
%   maxfun = 75;
%   maxiter = 75;
%   maxfun = 1000;
%   maxiter = 1000;
%   qf = 100;

% Expensive cost function parameters
%   T = 1;
%   Tfactor = .75;
%   numsteps = 10;
%   maxfun = 200;
%   maxiter = 200;
%   qf = 20;
%   schedule = 'regular';

% In case the defaults were gathered from calling: AnnealSet('Anneal'):
if ischar(maxfun)
   maxfun = eval(maxfun);
end
if ischar(maxiter)
   maxiter = eval(maxiter);
end

switch printtype
case {'none','off'}
   prnt = 0;
case 'iter'
   prnt = 2;
case 'final'
   prnt = 1;
case 'simplex'
   prnt = 3;
   clc
   formatsave = get(0,{'format','formatspacing'});
   format compact
   format short e
case 'best'
   prnt = 4;
case 'anim'
   prnt = 5;
otherwise
   prnt = 1;
end


% Convert to inline function as needed.
funfcn = fcnchk(funfcn,length(varargin));

if ~strcmp(fname,'off') & exist([fname,'.mat'])  % if savefile exists 
  load(fname);                                   %   load it
  % create old if doesn't exist
  if exist('old') ~= 1                              
    clear old
    old.xbest = {};
    old.fbest = [];
    old.newincl = 0;
  end
  % create xbest, etc, if doesn't exist
  if ~exist('xbest') 
    xbest = {};
    fbest = [];
  end
  % if xbest existed but been included in old, include it
  if ~old.newincl & ~isempty(xbest)
    old.xbest = {xbest, old.xbest{:}};
    old.fbest = [fbest; old.fbest(:)];
    [old.fbest, index] = sort(old.fbest);
    [old.xbest{:}] = deal(old.xbest{index});
  end
  % reset xbest etc and save 
  old.newincl = 0;
  xbest = {};
  fbest = [];
  save(fname,'fbest','xbest');
else
  old.xbest = {};
  old.fbest = [];
  old.newincl = 0;
end
  
v = [];
fv = [];
xbest = [];
fbest = Inf;

% if schedule is a string all other variables are used to determine schedule
%    including T, Tfactor, numsteps, maxfun, maxiter, and qfactor
% if schedule is a vector it denotes temperature at each step
%    maxfun, maxiter, are used and if schedule(end) = 0 qfactor is also used
%    however T and Tfactor and numsteps are not used
% if schedule is a three column matrix then first column is temperature, 
%    second column is maxfun, third column is maxiter.  In this case
%    none of maxfun, maxiter, qfactor, T, Tfactor, numsteps are used

% T=0 recovers the Nelder-Mead simplex search method

% Create schedule 
n_restart = 0;
y_restart = 1;
if isstr(schedule)  % schedule specified by other variables
  if strcmp(lower(schedule),'quench');
     schedule = [0, maxfun, maxiter, n_restart];
  else
    schedule = zeros(numsteps,4);
    for step = 1:numsteps
      schedule(step,:) = [T, maxfun, maxiter, (mod(step,restart)==0)];
      T = T*Tfactor; 
    end
    if qfactor > 0
      schedule = [schedule; 0, qfactor*maxfun, qfactor*maxiter,y_restart];
      numsteps = numsteps + 1;
    end
  end
elseif min(size(schedule)) == 1 & size(schedule,2) ~= 3
  numsteps = max(size(schedule));
  schedule = [schedule(:), ones(numsteps,1)*[maxfun, maxiter, n_restart]];
  for step = 1:numsteps
    schedule(step,4) = (mod(step,restart)==0);
  end
  if schedule(end,1) == 0  % quench max's may differ
    schedule(end,:) = [0, qfactor*maxfun, qfactor*maxiter, y_restart];
  end
else
  if size(schedule,2) ~= 3
    error('schedule must be string, vector, or three columm matrix');
  end
end
schedule(1,4) = n_restart;

% loop over schedule
numsteps = size(schedule,1);
for step = 1:numsteps
  if schedule(step,4)
    v = [];
    fv = [];
    x = xbest;
    disp('RESTARTING ....');
  end
  [v, fv, xbest, fbest]=sasearch(funfcn, ...
      schedule(step,1), ... % T
      x, v, fv, xbest, fbest, old, prnt, tolx, tolf, ...
      schedule(step,2), ... % maxfun
      schedule(step,3), ... % maxiter
      fname, clk0, varargin{:});
  disp(sprintf(['  step # %d of %d: (T=%f, maxfun=%d, maxiter=%d): ', ...
               'best = %f'],step,numsteps,...
               schedule(step,1), schedule(step,2), schedule(step,3), fbest));
  disp(['Total Elapsed Time = ',ntime(clock,clk0)]);
end

% Save best
old.xbest = {xbest, old.xbest{:}};
old.fbest = [fbest; old.fbest(:)];
[old.fbest, index] = sort(old.fbest);
[old.xbest{:}] = deal(old.xbest{index});
old.newincl = 1;
if ~strcmp(fname,'off')
  save(fname,'fbest','xbest', 'old');
end

%set output variables
x = xbest;
fval = fbest;
exitflag = [];

%reset format, if necessary
if prnt == 3,
   % reset format
   set(0,{'format','formatspacing'},formatsave);
end

%-------------------------------------------------------------%

function [v, fv, xbest, fbest] = ...
     sasearch(funfcn, T, x, v, fv, xbest, ... 
     fbest, old, prnt, tolx, tolf, maxfun, maxiter, ...
     fname, clk0, varargin);
%this funciton performs the SA random walk at the given temperature

% TO MAKE TO BE THE SAME AS _NRC_ 
%    (1) Change stopping criterion
%    (2) Add to keep track of xbest, fbest
%    (3) Uncomment out parts to add inside/outside shrink

%DEBUG:
%xinit = x;

% Initialize parameters
rho = 1; chi = 2; psi = 0.5; sigma = 0.5;

n = prod(size(x));
onesn = ones(1,n);
two2np1 = 2:n+1;
one2n = 1:n;

itercount = 1;
func_evals = 0;
how = 'initial';
exitflag = 1;

%make the simplex if it hasn't been done already
if isempty(v)
  % Set up a simplex near the initial guess.
  xin = x(:); %Force xin to be a column vector
  v = zeros(n,n+1); 
  fv = zeros(1,n+1);
  v(:,1) = xin; %Place input guess in the simplex! (credit L.Pfeffer Stanford)
  %SUPERFLUOUS: x(:) = xin;    % Change x to the form expected by funfcn 
  fv(:,1) = feval(funfcn,x,varargin{:}); 
  if fv(:,1) < fbest
    [fbest,xbest] ...  
         = procbest(fv(:,1),x,old,prnt,fname,clk0);
  end

  % Following improvement suggested by L.Pfeffer at Stanford
  usual_delta = 0.05;             % 5 percent deltas for non-zero terms
  zero_term_delta = 0.00025;      % Even smaller delta for zero elements of x
  for j = 1:n
     y = xin;
     if y(j) ~= 0
        y(j) = (1 + usual_delta)*y(j);
     else 
        y(j) = zero_term_delta;
     end  
     v(:,j+1) = y;
     x(:) = y; 
     f = feval(funfcn,x,varargin{:});
     if f < fbest
       [fbest,xbest] ...
            = procbest(f,x,old,prnt,fname,clk0);
     end
     fv(1,j+1) = f;
  end
  func_evals = n+1;
end     

%sort and print out starting information, if requested
[fv,j] = sort(fv);
v = v(:,j);
if prnt == 2 | prnt == 5
   disp(' ')
   header = ' Iteration   Func-count     min f(x)         Procedure';
   disp(header)
   disp([sprintf(' %5.0f        %5.0f     %12.6g         ', itercount, ...
        func_evals, fv(1)), how]) 
elseif prnt == 3
   clc
   formatsave = get(0,{'format','formatspacing'});
   format compact
   format short e
   disp(' ')
   disp(how)
   v
   fv
   func_evals
end

% Main algorithm
% Iterate until the diameter of the simplex is less than tolx
%   AND the function values differ from the min by less than tolf,
%   or the max function evaluations are exceeded. 
%   (Cannot use OR instead of AND.)

while func_evals < maxfun & itercount < maxiter
   % add noise and sort so v(:,1) has the lowest (noisy) function value 
   % (might move to below stopping criteria so that stopping criteria is
   % with exact values (chance nfv to fv) -- always sorted by exact values 
   % at this point)
   nfv = fv - T*log(rand(1,n+1));
   [nfv,j] = sort(nfv);
   fv = fv(:,j);
   v = v(:,j);

   % stopping criterion using noisy function values
   if max(max(abs(v(:,two2np1)-v(:,onesn)))) <= tolx & ...
         max(abs(nfv(1)-nfv(two2np1))) <= tolf
      [fv,j] = sort(fv);
      v = v(:,j);
      break
   end

   % stopping criterion from NRC, using noisy function values
   %tolr = 2*abs(nfv(n+1)-nfv(1))/(abs(nfv(n+1)) + abs(nfv(1)));
   %if tolr < tolf
   %   [fv,j] = sort(fv);
   %   v = v(:,j);
   %   break
   %end

   how = '';
   
   % xbar = average of the n (NOT n+1) best points
   xbar = sum(v(:,one2n), 2)/n;

   % Compute the reflection point
   xr = (1 + rho)*xbar - rho*v(:,end);
   x(:) = xr; 
   fxr = feval(funfcn,x,varargin{:});
   if fxr < fbest
     [fbest,xbest] ...
        = procbest(fxr,x,old,prnt,fname,clk0);
   end
   func_evals = func_evals+1;
   nfxr = fxr + T*log(rand); 

   % if value at reflected point is smaller than any in simplex ...
   if nfxr < nfv(:,1)
      % Calculate the expansion point (reflect farther)
      xe = (1 + rho*chi)*xbar - rho*chi*v(:,end);
      x(:) = xe; 
      fxe = feval(funfcn,x,varargin{:});
      if fxe < fbest
        [fbest,xbest] ...
            = procbest(fxe,x,old,prnt,fname,clk0);
      end
      func_evals = func_evals+1;
      nfxe = fxe + T*log(rand);

      % if value at expansion point is even less than value at reflected ...
      if nfxe < nfxr
         v(:,end) = xe;
         fv(:,end) = fxe;
         nfv(:,end) = nfxe;
         how = 'expand';
      else
         v(:,end) = xr; 
         fv(:,end) = fxr;
         nfv(:,end) = nfxr;
         how = 'reflect';
      end

   else % nfv(:,1) <= nfxr (value of reflected point not smaller than smallest)
      % if reflected value smaller than all other n (not n+1) values
      if nfxr < nfv(:,n)
         v(:,end) = xr; 
         fv(:,end) = fxr;
         nfv(:,end) = nfxr;
         how = 'reflect';
      else % nfxr >= nfv(:,n) 
         % Perform contraction (don't reflect as far)
         % if reflected value less than n+1 value (reflected from value)
         if nfxr < nfv(:,end)
            % Perform an outside contraction
            xc = (1 + psi*rho)*xbar - psi*rho*v(:,end);
            x(:) = xc; 
            fxc = feval(funfcn,x,varargin{:});
            if fxc < fbest
              [fbest,xbest] ...
                 = procbest(fxc,x,old,prnt,fname,clk0);
            end
            func_evals = func_evals+1;
            nfxc = fxc + T*log(rand);
 
            % if contracted value less than refected value
            if nfxc <= nfxr %(might change this to nfxc <= fxc + noise)
               v(:,end) = xc; 
               fv(:,end) = fxc;
               nfv(:,end) = nfxc;
               how = 'contract outside';
            else
               % perform a shrink

               % UNCOMMENT TO MAKE THE SAME AS NRC
               % v(:,end) = xr;
               % fv(:,end) = fxr;
               % nfv(:,end) = nfxr;

               %disp(sprintf('outside shrink at %d',itercount+1));
               %xinit
               
               how = 'shrink'; 
            end
         else  % reflected value greater than n+1st (reflected from value)
            % Perform an inside contraction
            xcc = (1-psi)*xbar + psi*v(:,end);
            x(:) = xcc;
            fxcc = feval(funfcn,x,varargin{:});
            if fxcc < fbest
              [fbest,xbest] ... 
                 = procbest(fxcc,x,old,prnt,fname,clk0);
            end
            func_evals = func_evals+1;
            nfxcc = fxcc + T*log(rand);

            if nfxcc < nfv(:,end) %might change this
               v(:,end) = xcc;
               fv(:,end) = fxcc;
               nfv(:,end) = nfxcc;
               how = 'contract inside';
            else
               % perform a shrink
 
               %disp(sprintf('inside shrink at %d',itercount+1));
               %xinit
               
               how = 'shrink';
            end
         end
         if strcmp(how,'shrink')
         %use "strmatch('shrink',how)" if adding inside/outside
            for j=two2np1
               v(:,j)=v(:,1)+sigma*(v(:,j) - v(:,1));
               x(:) = v(:,j); 
               fv(:,j) = feval(funfcn,x,varargin{:});
               if fv(:,j) < fbest
                 [fbest,xbest] ...
                    = procbest(fv(:,j),x,old,prnt,fname,clk0);
               end
            end
            func_evals = func_evals + n;
         end
      end
   end
   itercount = itercount + 1;

   % sort and print out
   [fv,j] = sort(fv);
   v = v(:,j);
   if prnt == 2
      disp([sprintf(' %5.0f        %5.0f     %12.6g         ', itercount, ...
           func_evals, fv(1)), how]) 
   elseif prnt == 3
      disp(' ')
      disp(how)
      v
      fv
      func_evals
   elseif prnt == 5
      disp([sprintf(' %5.0f        %5.0f     %12.6g         ', itercount, ...
           func_evals, fv(1)), how]) 
      hold on
      plot(v(1,:),v(2,:),'*r');
      disp('Press Any Key');
      pause 
      plot(v(1,:),v(2,:),'*k');
   end  
end   % while

% -------------------------------------------------------------------%

%output.iterations = itercount;
%output.funcCount = func_evals;
%output.algorithm = 'Nelder-Mead simplex direct search';
%
%if func_evals >= maxfun 
%   if prnt > 0
%      disp(' ')
%      disp('Exiting: Maximum number of function evaluations has been exceeded')
%      disp('         - increase MaxFunEvals option.')
%      msg = sprintf('         Current function value: %f \n', fval);
%      disp(msg)
%   end
%   exitflag = 0;
%elseif itercount >= maxiter 
%   if prnt > 0
%      disp(' ')
%      disp('Exiting: Maximum number of iterations has been exceeded')
%      disp('         - increase MaxIter option.')
%      msg = sprintf('         Current function value: %f \n', fval);
%      disp(msg)
%   end
%   exitflag = 0; 
%else
%   if prnt > 0
%      convmsg1 = sprintf([ ...
%         '\nOptimization terminated successfully:\n',...
%         ' the current x satisfies the termination criteria using OPTIONS.TolX of %e \n',...
%         ' and F(X) satisfies the convergence criteria using OPTIONS.TolFun of %e \n'
%          ],options.TolX, options.TolFun);
%      disp(convmsg1)
%      exitflag = 1;
%   end
%end


function [fbest,xbest]=procbest(fx,x,old,prnt,fname,clk0);
fbest = fx;
xbest = x;
if ~strcmp(fname,'off')
  save(fname,'fbest','xbest','old');
end
if prnt == 4 | prnt == 3
  disp(['best cost = ', num2str(fbest), ' : ',ntime(clock,clk0)])
end 

function s = ntime(t,t0)
% NTIME -- displays time t in seconds in minutes, hours, days appropriately

if nargin == 2
  t = etime(t,t0);
end

if t > 86400
 s = sprintf('%f days',t/86400');
elseif t > 3600
 s = sprintf('%f hours',t/3600);
elseif t > 60
 s = sprintf('%f minutes',t/60);
else
 s = sprintf('%f seconds',t);
end


