function [eigen,phi] = numericfloquet(ode,tspan,yo,odeopts,disc,varargin)
% NUMERICFLOQUET Estimate Floquet multipliers & state-transition matrix.
%   EIGEN = NUMERICFLOQUET(ODE,TSPAN,YO) with TSPAN = [T0 TFINAL] calculates
%   the eigenvalues of the state-transition (fundamental) matrix for a 
%   continuous system by tracking perturbations about the inital condition, YO. 
%   The continuous system's dervitive, y'=f(t,y), is defined in the function 
%   specified by the function handle ODE. The solver is ODE45.
%
%   EIGEN = NUMERICFLOQUET(ODE,TSPAN,YO,ODEOPTS) pass options created with 
%   ODESET to the ode45 solver.
%
%   EIGEN = NUMERICFLOQUET(ODE,TSPAN,YO,ODEOPTS,DISC) where DISC is a 1 x 2 
%   cell array, {DISCPRE,DISCPOST} of function handles to discrete functions 
%   that are applied before and after the integration of the continuous 
%   equations. Pass an empty argument, [], to not use this option. The discrete 
%   functions must only require the state as input. Use anonymous functions if 
%   additional parameters are required.
%
%   EIGEN = NUMERICFLOQUET(ODE,TSPAN,YO,ODEOPTS,DISC,...) any additional 
%   arguments will be passed directly to the ODE45 solver.
%
%   [EIGEN,PHI] = NUMERICFLOQUET(...) returns an estimate of the state-
%   transition matrix, PHI.
%
%   A warning is provided if the initial condition does not agree with the
%   final state after integration. In other words, it is not an equilibrium
%   or periodic solution. To disable this warning, execute the following
%   command: warning('off', 'numericfloquet:refsol')
%
%   This method is not as robust as the ANALYTICALFLOQUET, which may be used to
%   calculate the state-transition matrix by integrating the variational 
%   equations from the Jacobian evaluated along the trajectory, Y.
% 
%   Input
%       ODE      | 1 x 1 | function handle | e.g. @dynamics
%       TSPAN    | 1 x 2 | double          | simulation time [TO TFINAL]
%       YO       | n x 1 | double          | initial state
%       ODEOPTS  | 1 x 1 | struct          | ode options, use: odeset
%       DISC     | 1 x 2 | cell array      | handles to discrete functions
%       VARARGIN |   ?   |                 | passed directly to odesolver       
%
%   Output
%       EIGEN | n x 1 | double | Eigenvalues
%       PHI   | n x n | double | State transition matrix
%   
%   Example        
%       numericfloquetex; % Run the example script
%
%   Reference
%     [1] Jordan, DW, and Smith, P, "Nonlinear Ordinary Differential Equations:
%         An Introduction to Dynamical Systems," 3rd Edition, Oxford University 
%         Press, 1999. Chapter 9. 
%
%   See also NUMERICFLOQUETEX, ANALYTICALFLOQUET, ODE45.

% Created by: Jesse Norris
%             Kevin P. Granata Musculoskeletal Biomechanics Laboratory
%             http://jesse.musculoskeletaldynamics.com
% Last Revision: 5 July 2007

% Handle input arguments
  if (nargin == 3)
      odeopts = odeset;
  end;
      

% Apply a discrete transfer function
  pre = (nargin > 4) && ~isempty(disc) && isa(disc{1},'function_handle');
  if pre
      y1 = feval(disc{1},yo);
  else
      y1 = yo;
  end
     
% Integrate unperturbed solution 
  [tf,yf] = ode45(ode,tspan,y1,odeopts,varargin{:});     
  yfo     = yf(end,:)';

% Apply discrete transfer function post continuous time integration  
  post = (nargin > 4) && ~isempty(disc) && isa(disc{2},'function_handle');
  if post
      yfo = feval(disc{2},yfo);  
  end
  
% If max(abs(F(yo)-yo)) > 1e-6, provide a warning that F(yo)-yo 
% may not be a reference solution            
  if max(abs(yfo(:)-yo(:))) > 1e-6  
      warnstr = ['yo may not be a periodic solution'...
                 '\n\tF(yo)-yo =\n'...
                 sprintf('\t\t%.3e\n',yfo(:)-yo(:))];                 
      warning('numericfloquet:refsol',warnstr);
  end

% Delta disturbance matrix, note the size of the perturbations
  delta = 1e-7*eye(length(yo));
  
% Initialize matrix for output response to disturbace  
  yfMinusYfo = zeros(size(delta)); % F(yo+delta) - yfo 
     
% Apply disturbance to each yo  
  for ind = 1:length(yo)
      
      % Apply a discrete transfer function
      if pre 
          y1 = feval(disc{1},yo+delta(:,ind));
      else
          y1 = yo+delta(:,ind);
      end

      % Solve F(yo+delta)
      [tf,yf] = ode45(ode,tspan,y1,odeopts,varargin{:});   
      yf      = yf(end,:)';
      
      % Apply discrete transfer function post continuous time integration
      if post
          yf = feval(disc{2},yf); 
      end

      % Compute F(yo+delta) - yfo
      yfMinusYfo(:,ind) = yf(:)-yfo(:); 
  end        

% Estimate state-transition matrix = (F(yo+delta)-yfo)*inv(delta)
  phi = yfMinusYfo/delta;
  
% Compute eigenvalues of state-transition matrix (Floquet multipliers for 
% periodic solutions)   
  eigen = eig(phi);     
  
end % END numericfloquet