[yout,tout]=check_3D_step_mcg(t0,tfinal,y0,tol);

function [yout,tout]=check_3D_step_mcg(t0,tfinal,y0,tol);


% Does forward integration ONE STEP ONLY for a 3D walker with
% 4 DOF. Assumes that relevant init file has been called.
% This file computes the "stride function", the next
% step ICs as functions of the previous ones.
% The only difference between this file and
% int_3D_step_mcg.m is that this one computes a few
% extra things about the gait, like step period, etc.
% See that file for comments.

global g gam m p34 pc3 pc4 p4f Icm3 Icm4 hspr hdamp sspr sdamp
% don't put global h up here, since a different h is used!

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%       C.B. Moler, 3-25-87, 10-5-91, 6-3-93.
%       Copyright (c) 1984-93 by The MathWorks, Inc.
%       Modified for walking simulations
%       by Mariano Garcia ([e-mail])
%%%%%%%%%%%%%%%%%%%%%%%%%% Program begins  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Initialization
% The Fehlberg coefficients:
beta  = [ [    1      0      0     0      0    0]/4
          [    3      9      0     0      0    0]/32
          [ 1932  -7200   7296     0      0    0]/2197
          [ 8341 -32832  29440  -845      0    0]/4104
          [-6080  41040 -28352  9295  -5643    0]/20520 ]';
gamma = [ [902880  0  3953664  3855735  -1371249  277020]/7618050
          [ -2090  0    22528    21970    -15048  -27360]/752400 ]';
pow = 1/5;
f = zeros(length(y0),6);
t = t0;
k=0;

hmax = (tfinal - t)/16;
h = hmax/8;
y = y0(:);
tau = tol * max(norm(y, 'inf'), 1);
clf reset;
% hf1 is height of swing foot above ramp surface
% hfdot1 is the vertical velocity of the swing foot
hf1=0;
hfdot1=1;


%%%%%%%%%%%%%%%%%%%%%  Main Loop Begins  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
while k<1

  while  (hfdot1>0) & k < 1
         % Compute the slopes
   junk = yderivs_3D(t,y);
   f(:,1) = junk(1:8);
   hf1=junk(9);
   hfdot1 = junk(10);
   for j = 1:5
   junk = yderivs_3D(t,y+h*f*beta(:,j));
   f(:,j+1) = junk(1:8);
   end


% Estimate the error and the acceptable error
   delta = norm(h*f*gamma(:,2),'inf');
   tau = tol*max(norm(y,'inf'),1.0);

	% Update the solution only if the error is acceptable
   if delta <= tau
      t = t + h;
      y = y + h*f*gamma(:,1);

   end;

        % Update the step size

     if delta ~= 0.0
      h = min(hmax, 0.8*h*(tau/delta)^pow);
     end

end;

  while  ((hf1+h*hfdot1<0) | hfdot1 < 0) & k < 1
         % Compute the slopes
   junk = yderivs_3D(t,y);
   f(:,1) = junk(1:8);
   hf1=junk(9);
   hfdot1 = junk(10);
   for j = 1:5
   junk = yderivs_3D(t,y+h*f*beta(:,j));
   f(:,j+1) = junk(1:8);
   end

% Estimate the error and the acceptable error
   delta = norm(h*f*gamma(:,2),'inf');
   tau = tol*max(norm(y,'inf'),1.0);

        % Update the solution only if the error is acceptable
   if delta <= tau
      t = t + h;
      y = y + h*f*gamma(:,1);

   end;

        % Update the step size

     if delta ~= 0.0
      h = min(hmax, 0.8*h*(tau/delta)^pow);
     end

end;

while  (hf1+h*hfdot1>0) & k<1
         % Compute the slopes
   junk = yderivs_3D(t,y);
   f(:,1) = junk(1:8);
   hf1=junk(9);
   hfdot1 = junk(10);
   for j = 1:5
   junk = yderivs_3D(t,y+h*f*beta(:,j));
   f(:,j+1) = junk(1:8);
   end

% Estimate the error and the acceptable error
   delta = norm(h*f*gamma(:,2),'inf');
   tau = tol*max(norm(y,'inf'),1.0);

        % Update the solution only if the error is acceptable
   if delta <= tau
      t = t + h;
      y = y + h*f*gamma(:,1);

   end;

% get new hf1, hf1dot
junk = yderivs_3D(t,y);
   hf1=junk(9);
   hfdot1 = junk(10);


        % Update the step size

     if delta ~= 0.0
      h = min(hmax, 0.8*h*(tau/delta)^pow);
     end

end;
horig=h;
%%%%%%%%%%%%%% Nearing Heelstrike %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% now we are less than h `seconds' away from strike
% do Newton's method to get to kneestrike
% or Poincare's method or whatever it is.
% assume we want to find the right h to get us right to heelstrike
% swing foot is at height junk(9) moving with vertical
% velocity junk(10). 

	q=0;

 while abs(hf1) > tol/1000 & k<1
h=(-hf1/hfdot1);
% Compute the slopes
   junk = yderivs_3D(t,y);
   f(:,1) = junk(1:8);
   for j = 1:5
    junk = yderivs_3D(t,y+h*f*beta(:,j));
    f(:,j+1) = junk(1:8);
   end

        % Update the solution
      t = t + h;
      y = y + h*f*gamma(:,1);

junk = yderivs_3D(t,y);
   hf1=junk(9);
   hfdot1=junk(10);

	  q=q+1;
 end;



%%%%%%%%%%%%%%%%%%%%%%%%%%%% Heelstrike %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if k<1
ynew = heelstrike_mcg(y);
hf1 = 0;
hfdot1=1;
  y = ynew';
k=k+1;

  h=horig/3;

end;
%%%%%%%%%%%%%%%%%%%%%%%%% End Main Loop %%%%%%%%%%%%%%%%%%%%%%%%%%%%%

end;
yout=y;
tout = t;