% -----------------------------------------------------------------------------
% AnalyzeHexRun.m
% -----------------------------------------------------------------------------
%
%  This script analyzes the provided spatial hexapod data and returns a summary
%
% -----------------------------------------------------------------------------
%
%  function result = ...
%      AnalyzeHexRun( data, threshold, runlength, tolerance );
%
% -----------------------------------------------------------------------------
%  Input Arguments :
%
%     data      : The trajectory data as returned by LoadSimSectData() on the
%                  Bipedal SLIP data output.
%     threshold : Number of stable apexes from the end of run for success
%     runlength : Expected length of the run. Assumed unstable if actual is
%                 different. 
%     tolerance : Stability tolerance in the magnitude of ydot and z
%
% -----------------------------------------------------------------------------
%  Outputs :
%
%     result : A struct with the results of the analysis. The following 
%              fields are returned:
%
%          result.stable : Flag indicating whether the run converges or not
%          result.z      : If stable, the apex height of the fixed point
%          result.xdot   : If stable, the apex x speed of the fixed point
%          result.ydot   : If stable, the apex y speed of the fixed point
%          result.speed  : If stable, apex velocity of the fixed point
%          result.roll   : If stable, apex roll
%          result.pitch  : If stable, apex pitch
%          result.rolld  : If stable, apex roll velocity
%          result.pitchd : If stable, apex pitch velocity
%
% -----------------------------------------------------------------------------
% $Id: AnalyzeHexRun.m,v 1.1.1.1 2003/10/28 20:05:59 ulucs Exp $
% -----------------------------------------------------------------------------

function result = ...
    AnalyzeHexRun( data, threshold, runlength, tolerance );

% Extract the transition points
indices = FindHexPoints( data );

  % Extract the relevant vectors
apex.t      = data.time( indices.apex );

apex.z      = data.r( indices.apex,3 );
apex.xdot   = data.rd( indices.apex,1 );
apex.ydot   = data.rd( indices.apex,2 );
apex.speed  = sqrt( apex.xdot.^2 + apex.ydot.^2 );
apex.roll   = data.roll( indices.apex );
apex.pitch  = data.pitch( indices.apex );
apex.rolld  = data.rolld( indices.apex );
apex.pitchd = data.pitchd( indices.apex );

% Check whether the length of the run is sufficient
if ( length( apex.t ) ~= runlength )
  result.stable = -1;
  return;
end

fullrange = (( runlength-threshold ):runlength);
rangea = (( runlength-threshold ):2:runlength);
rangeb = (( runlength-threshold+1 ):2:runlength);
finalspeeda = mean( apex.speed( rangea ) );
finalspeedb = mean( apex.speed( rangeb ) );
finalza = mean( apex.z( rangea ) );
finalzb = mean( apex.z( rangeb ) );

result.z      = mean( apex.z( fullrange ) );
result.xdot   = mean( apex.xdot( fullrange ) );
result.ydot   = mean( apex.ydot( fullrange ) );
result.speed  = mean( apex.speed( fullrange ) );
result.roll   = mean( apex.roll( fullrange ) );
result.pitch  = mean( apex.pitch( fullrange ) );
result.rolld  = mean( apex.rolld( fullrange ) );
result.pitchd = mean( apex.pitchd( fullrange ) );

disp( sprintf('speeda:%f, speedb:%f, za:%f, zb:%f', ...
              finalspeeda, finalspeedb, finalza, finalzb ) );

% Check whether both ends of the period two orbit are stable or not.
if ( abs( max( apex.speed( rangea ) - finalspeeda ) ) > 10 * tolerance | ...
     abs( max( apex.speed( rangeb ) - finalspeedb ) ) > 10 * tolerance )
  result.stable = 0;
  return
end

if ( abs( max( apex.z( rangea ) - finalza ) ) > tolerance | ...
     abs( max( apex.z( rangeb ) - finalzb ) ) > tolerance )
  result.stable = 0;
  return
end

if ( abs( finalspeeda - finalspeedb ) < 10 * tolerance & ...
     abs( finalza - finalzb) < tolerance )
     result.stable = 1;
else
     result.stable = 2;
end

