function [eigen,phi,yf] = analyticalfloquet(ode,tspan,yo,odeopts,nsigma,varargin)
% ANALYTICALFLOQUET Estimate Floquet multipliers & state-transition matrix.
%   EIGEN = ANALYTICALFLOQUET(ODE,TSPAN,YO) with TSPAN = [T0 TFINAL] calculates
%   the eigenvalues of the state-transition (fundamental) matrix for a 
%   continuous system by integrating the variational equations (jacobian) along 
%   the solution from inital condition, YO. The continuous system's and 
%   variational equation's dervitives, Y'=f(t,Y), are defined in the function 
%   specified by the function handle ODE. The solver is ODE45.
% 
%   For a system of dimension n, Y'=f(t,Y) should include a total of (n+n^2) 
%   equations. n equations for the evolution of the trajectory from Y0, and n^2 
%   equations describing the evolution of the variational equations. (see
%   example of Lorenz equations below)
%
%   EIGEN = ANALYTICALFLOQUET(ODE,TSPAN,YO,ODEOPTS) pass options created with 
%   ODESET to the ode45 solver. Pass an empty argument, [], to use default ode 
%   options.
%
%   EIGEN = ANALYTICALFLOQUET(ODE,TSPAN,YO,ODEOPTS,NSIGMA) where NSIGMA is a 
%   n x 1 column vector that is normal to the Poincare section that halted
%   integration. This was designed for estimating evolution in hybrid discrete-
%   continuous systems, where the discrete event is applied as a state-dependent 
%   function. Pass an empty argument, [], to not use this option.
%
%   EIGEN = ANALYTICALFLOQUET(ODE,TSPAN,YO,ODEOPTS,NSIGMA,...) any additional 
%   arguments will be passed directly to the ODE45 solver.
%
%   [EIGEN,PHI] = ANALYTICALFLOQUET(...) returns an estimate of the state-
%   transition matrix, PHI.
%
%   [EIGEN,PHI,YF] = ANALYTICALFLOQUET(...) returns the state at the end of
%   the integration.
%
%   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', 'analyticalfloquet:refsol')
%
%   For example of ODE, see example/dlorenz.m         
% 
%   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
%       NSIGMA   | n x 1 | double          | vector normal to Poincare section
%       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. 
%
%     [2] Hiskens, IA, "Stability of hybrid system limit cycles: application to 
%         the compass gait biped robot," In 40th IEEE Conference on Decision and 
%         Control, 2001, pp. 774-779.
%
%   See also NUMERICFLOQUETEX.

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

% Handle input arguments
  if (nargin == 3) || isempty(odeopts)
      odeopts = odeset;
  end;      
  
% Append yo with identity matrix (initial condition for state-transition matrix)
  n  = length(yo);  
  yo = [yo(:); reshape(eye(n),[],1)];
     
% Integrate unperturbed solution & variational equations   
  [tf,yf] = ode45(ode,tspan,yo,odeopts,varargin{:});     
  yf      = yf(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(yf(1:n)-yo(1:n))) > 1e-6  
      warnstr = ['yo may not be a periodic solution'...
                 '\n\tF(yo)-yo =\n'...
                 sprintf('\t\t%.3e\n',yf(1:n)-yo(1:n))];                 
      warning('analyticalfloquet:refsol',warnstr);
  end
  
% Estimate of state-transition matrix 
  phi = reshape(yf(n+1:end),n,n);
  
% Account for a Poincare section defined by the vector normal, NSIGMA
  if (nargin > 4) && ~isempty(nsigma)
      df  = feval(ode,tf(end),yf(1:n),varargin{:});
      df  = df(1:n);
      R   = eye(n)-df*nsigma'/(df'*nsigma);
      phi = R*phi;
  end;
  
% Compute eigenvalues of state-transition matrix (Floquet multipliers for 
% periodic solutions)   
  eigen = eig(phi);     
  
% Return state at end of integration
  yf = yf(1:n);
  
end % END analyticalfloquet
