% ---------------------------------------------------------------
%
% DrawTorqueLimits
%
%  Draws shaded regions indicating the torque limits for the 
% specified index
%

function DrawTorqueLimits( vars, index );

% The torquecolors is a vector with the colors of torque patches for
% shades encoding the number of legs on the ground
torquecolors = [ 1 1 1; 0.8 0.8 1; 0.6 0.6 1; 0.4 0.4 1; ...
                 1 1 1; 0.8 1 0.8; 0.6 1 0.6; 0 1 0 ];

% Find where tripod transitions occur
transitions = find( diff( vars.curtripod ) ) + 1;

touchdowncount = CountLegs( vars.chart, vars.curtripod);
touchdowns = find( diff( touchdowncount ) ) + 1;
transitions = sort( [ 1; transitions; touchdowns; length(vars.time)] );

% Compute the legs of the current tripod
leftlegs =  [ 3 1 5 ];
rightlegs = [ 0 4 2 ];

cla;
hold on;
for i = 1:length(transitions)-1,
  range = transitions(i):transitions(i+1);
  numlegs = touchdowncount(transitions(i));
  tripodflag = vars.curtripod(transitions(i));
  color = torquecolors( numlegs + 4*tripodflag + 1, :);
  fill( [vars.time(range); flipud(vars.time(range))], ...
        [vars.leftlimits( range, index ); ...
         flipud( vars.rightlimits( range, index ))], ...
        color );
  if ( tripodflag == 0 )
	curleg = leftlegs(index);
  else
	curleg = rightlegs(index);
  end
  plot( vars.time(range), vars.hiptorque(range, curleg+1), 'k');
  plot( vars.time(range), vars.taupsiorig(range, index), 'y');
  plot( vars.time(range), vars.taupsiproj(range, index), 'r');
end

axis tight;
% CountLegs : counts the number of legs touching the ground among 
% a particular set of legs
function count = CountLegs( chart, curtripod );

mask = curtripod * 21 + (1 - curtripod) * 42;

chart = bitand( chart, mask );

count = 0;
for i = 0:5;
  count = count + bitshift(bitand( chart, 2^i ), -i);
end

