% -----------------------------------------------------------------------------
% slip_vector_field.m
% -----------------------------------------------------------------------------
%
%  Returns the vector field for a planar SLIP. This is designed to be used with
% ODE functions that detect events.
%
% -----------------------------------------------------------------------------
%
%  function xdot = slip_vector_field( t, x, flag, params )
%
% -----------------------------------------------------------------------------
%  Input Arguments :
%
%    t : time
%    x : State vector, defined as follows
%        1: y
%        2: z
%        3: ydot
%        4: zdot
%
%   flags : One of the following strings:
%        ''       : Return the vector field
%        'init'   : Perform initialization
%        'events' : Return event functions
%
%   params : Struct with the following fields :
%        m_body  : body mass
%        xi_rest : leg rest length
%        k_slip  : spring constant for hooke's law spring
%        gravity : gravitational acceleration ( > 0 )
%        y_vf    : y coordinate of the virtual foot
%        xi_td   : touchdown leg length
%        psi_td  : touchdown leg angle
%        phase   : current chart (0: flight, 1: stance)
%
% -----------------------------------------------------------------------------
%  Outputs :
%
%    Depending on the value of input 'flags', this function returns 
%  the appropriate values as in ODE functions.
%
% -----------------------------------------------------------------------------
% $Id: slip_vector_field.m,v 1.1.1.1 2003/10/28 20:05:59 ulucs Exp $
% -----------------------------------------------------------------------------

function varargout = slip_vector_field( t, x, flag, params )

  switch ( flag )
   case ''
    varargout{1} = slip_f( t, x, params );
   case 'init'
    [varargout{1:3}] = slip_init( params );
   case 'events'
    [varargout{1:3}] = slip_events( t, x, params );
   otherwise
    error(['Unknown flag ''' flag '''.']);
  end
    
    
% -----------------------------------------------------------------------------
% Initialization function

function [tspan,x0,options] = slip_init( params )

tspan = [0 0.1];
x0 = [0 0.2 1.0 0.0];
options = odeset('Events', 'on');
    
% -----------------------------------------------------------------------------
% Vector field function

function dxdt = slip_f( t, x, params )

  dxdt = zeros(4,1);
  
  y = x(1);
  z = x(2);
  ydot = x(3);
  zdot = x(4);
  
  slip = com_to_slip( x, params.y_vf );

  xi = slip(1);
  psi = slip(2);
  xidot = slip(3);
  psidot = slip(4);
  
  if ( params.phase == 0 )

    yddot = 0.0;
    zddot = - params.gravity;
  
  elseif ( params.phase == 1 )
  
    springforce = params.k_slip * (params.xi_rest - xi);
  
    yddot = -springforce * sin( psi ) / params.m_body;
    zddot = springforce * cos( psi ) / params.m_body - params.gravity;
  
  elseif ( params.phase == 2 )
  
    springforce = params.k_slip * (params.xi_rest - xi);
  
    yddot = -springforce * sin( psi ) / params.m_body;
    zddot = springforce * cos( psi ) / params.m_body - params.gravity;
  
  elseif ( params.phase == 3 )
  
    yddot = 0.0;
    zddot = - params.gravity;
  
  end
  
  dxdt(1) = ydot;
  dxdt(2) = zdot;
  dxdt(3) = yddot;
  dxdt(4) = zddot;

% -----------------------------------------------------------------------------
% Event functions

function [value, isterminal, direction] = slip_events( t, x, params )

  y = x(1);
  z = x(2);
  ydot = x(3);
  zdot = x(4);
  
  slip = com_to_slip( x, params.y_vf );
  
  xi = slip(1);
  psi = slip(2);
  xidot = slip(3);
  psidot = slip(4);
  
  if ( params.phase == 0 )

    value = z - params.xi_td * cos( params.psi_td );
    isterminal = 1;
    direction = -1;
  
  elseif ( params.phase == 1 )

    value = -((y - params.y_vf) * ydot + z * zdot) / xi;
    isterminal = 1;
    direction = -1;
  
  elseif ( params.phase == 2 )

    value = params.xi_rest - xi;
    isterminal = 1;
    direction = -1;
  
  elseif ( params.phase == 3 )

    value = zdot;
    isterminal = 1;
    direction = -1;
  
  end
  