function [Xnext,tani,xani] = cg_step(Xstart)
% function [Xnext,tani,xani] = cg_step(Xstart)
% 
% This function simulates the continuous dynamics
% of a passive compass gait walker beginning
% in the input state, "Xstart", until a collision 
% with the ground is detected.  It has 3 outputs:
%   Xnext : The first post-collision state detected
%   tani : a time vector (for animation)
%   xani : the states over time (for animation)
%
% State definition. Both Xstart and Xnext use
% "Goswami" convention for the 4 states:
%      X(1) = angle of non-stance (ns) foot, wrt +y axis (CCW)
%      X(2) = angle of stance (s) foot, wrt +y axis (CCW)
%      X(3) = dXin(1)/dt (ns foot, absolute ang. vel.)
%      X(4) = dXin(2)/dt (s foot, absolute ang. vel.)

% Time below may need to be longer, for certain initial
% conditions!  It is set to speed up run time right now.
tsim = 1;  % 1 second; much longer than actual step time
[tout,Xout] = ode45(@cg_eom,[0 tsim],Xstart);

% Find the PRE-collision state, just BEFORE swing leg hits ground:
[thit,Xpre,tani,xani] = cg_find_impact(tout,Xout);
    
Xpre = Xpre(:); % convert to a COLUMN vector

% Define Xpost, AFTER the collision: 1) swap legs, 2) ang momentum
Xnext = cg_impact(Xpre);  % pre-to-post collision transformation

