% ---------------------------------------------------------------
%
% DrawSlipMode
%
%  Draws shaded regions indicating the current SLIP mode for the 
% virtual SLIP

function DrawModeSequence( vars );

% Parameters for setup
modecolors = [1 1 0; 0.9 0.9 0; 0.8 0.8 0; 0.7 0.7 0];

keyframes = [];

% ---------------
% Setup the axes
cla;
handle = gca;

axis([0 vars.time(end) -2.4 1]);
set( handle, 'YTick', [] );

% ----------------------------------------------------------
% Draw the virtual SLIP mode as maintained by the controller

lastmode = vars.slipmode(1);
lastIndex = 1;

patch( [0 0 1 1], [2 3 3 2], modecolors(1,:) );
patch( [0 0 1 1], [2 3 3 2], modecolors(2,:) );
patch( [0 0 1 1], [2 3 3 2], modecolors(3,:) );
patch( [0 0 1 1], [2 3 3 2], modecolors(4,:) );

for ind = 2:length( vars.slipmode ),

  if ( vars.slipmode(ind) ~= lastmode )
	patch( [ vars.time(lastIndex) vars.time(lastIndex) ...
			 vars.time(ind) vars.time(ind)], ...
		   [ 0 1 1 0 ], ...
		   modecolors(lastmode+1, :) );
	lastmode = vars.slipmode(ind);
	lastIndex = ind;
	keyframes(end+1) = vars.time(ind);
  end
  
end;

patch( [ vars.time(lastIndex) vars.time(lastIndex) ...
		 vars.time(ind) vars.time(ind)], ...
	   [ 0 1 1 0 ], ...
	   modecolors(lastmode+1, :) );

set( handle, 'XTick', keyframes );

% ----------------------------------------------------------
% Draw the leg modes
DrawLegMode( vars, 0 );
DrawLegMode( vars, 1 );
DrawLegMode( vars, 2 );
DrawLegMode( vars, 3 );
DrawLegMode( vars, 4 );
DrawLegMode( vars, 5 );

% ----------------------------------------------------------
% Draw the current tripod
hold on;
plot( vars.time, 0.3*vars.curtripod + 0.1);

% Setup final touches
legend('tripod', 'ascent', 'descent', 'compression', 'decompression' );

grid;

% -------------------------------------------------------------------
% This internal function creates patches for the touchdown states of 
% a leg
function DrawLegMode( vars, leg_no );

legcolor = [0 0 1];

lastmode = bitand( vars.chart(1), 2^leg_no );
lastindex = 1;

ylocations = [3 1 5 0 4 2];

ymin = -0.05 - ylocations(leg_no+1)*0.4;
ymax = -0.35 - ylocations(leg_no+1)  *0.4;

for i = 2:length( vars.chart ),

  legmode = bitand( vars.chart(i), 2^leg_no );
  
  if ( legmode ~= lastmode )
	if ( lastmode ~= 0 )
	  patch( [ vars.time(lastindex) vars.time(lastindex) ...
			   vars.time(i) vars.time(i)], ...
			 [ ymin ymax ymax ymin ], ...
			 legcolor );
	end
	
	lastmode = legmode;
	lastindex = i;
  end
  
end;

if ( lastmode ~= 0 )
  patch( [ vars.time(lastindex) vars.time(lastindex) ...
		   vars.time(i) vars.time(i)], ...
		 [ ymin ymax ymax ymin ], ...
		 legcolor );
end

