function animate2(w,varargin)
% animate(w)
% animates the 2-d rimless wheel by w.
% animate(w, 'numsteps', N) automatically repeats the simulation N times (default 2).
% animate(w, x, stepindices) animates w with the given states given in x
% (rows of the state vector), and with stepindices containing a list
% of the number of frames in each step.
% animate(w, 'arrows', 1) also draws arrows for COM velocity
% animate(w, 'save', 1) animates w and saves each frame as an
% adobe illustrator file
% animate(w, 'frames', Nframes) draws a certain number of frames per step
% Art Kuo
parms = get(w,'parms'); alpha = parms.alpha;
%footn = 10; % how many segments to draw in the curved foot
debg = 0; % set to 1 to help debugging
numsteps = 2; saveflag = 0; arrowflag = 0; nframes = 16; % default values
x0 = []; stepindices = [];
if nargin == 0
error('animate: need a walk object as argument');
elseif nargin > 1 % optional arguments
% figure out if the second argument is an initial condition vector, or a
% bunch of rows of states
property_argin = varargin;
secondargument = property_argin{1};
if isa(varargin{1}, 'double') % second argument appears to be a number
if length(secondargument) == 4 % and it's a vector, meaning an initial condition
x0 = secondargument;
property_argin = property_argin(2:end);
elseif size(secondargument,1) > 1 %&& size(secondargument,2) == 4 % it's a matrix of states %<------------------
xs = varargin{1}; stepindices = varargin{2}; numsteps = length(stepindices); %<------------------
% bumps = varargin{3}; %<------------------
property_argin = property_argin(3:end);
else
error('animate: unknown second argument')
end
end
% Step through the optional arguments
while length(property_argin) >= 3,%<------------------
prop = property_argin{1};
val = property_argin{2};
property_argin = property_argin(3:end);
switch prop
case 'numsteps'
numsteps = val;
case 'save'
saveflag = val;
case 'arrows'
arrowflag = val;
case 'frames'
nframes = val;
end
end
end
if isempty(stepindices) % we've haven't been supplied with a bunch of states
[xe,te,xs,ts] = onestep(w, x0, 'anim', nframes); stepindices = length(xs);
end
xlen = length(xs);
% if length(stepindices) == 1 % there's just one step stored in xs
% stepindices = [xlen repmat(xlen-1, 1, numsteps-1)];
% startindex = repmat(1,numsteps,1); % extra frame
% endindex = repmat(xlen, numsteps, 1);
% elseif length(stepindices) > 1 % there's more than one step in xs
% endindex = cumsum(stepindices);
% startindex = [1 endindex-1];
% end
% Now numsteps contains the number of steps, stepindices
% contains the number of frames in each step, and
% startindex and endindex contain indices for each step
% Estimate range of walking
distance = (numsteps+1)*2*sin(alpha);
xlimit = [0 distance]-2*sin(alpha);
ylimit = [-0.05 2.05];
% arrow parameters
aang = pi/6; scale = 0.02; scale2 = 2; vx2 = 0.4; vy2 = 1.2;
% Initialize
clf; axis equal
%hold on
LW = 0.5;
bumps = get(w,'bumps');
% figPos = [500, 500, 300, 100];%[100, 100, 1800, 1800];
% fig1 = figure('Position', figPos);
fig1 = figure;
hold on; axis equal
h = drawmodel(w, xs(1,:), [0;0]); set(gcf,'Color','w'); axis off;
nom_steplen = 0.5910;
xlim([-2*nom_steplen (length(bumps)+3)*nom_steplen]); xlimTemp = xlim;
% line(xlim, [0 0],'Color','Black', 'LineWidth',LW); % do not plot a whole line
%find and draw bump
bump_len = 1.1*nom_steplen;
bump_index = find(bumps ~= 0); bump = bumps(bump_index(1));
bump_mag = cos(alpha-bump) - cos(alpha+bump);
coef = 0.95;
l_left_corner = [bump_index(1)*nom_steplen*coef; 0];
line([xlimTemp(1) l_left_corner(1)], [0 0],'Color','Black', 'LineWidth',LW);
u_left_corner = [bump_index(1)*nom_steplen*coef; bump_mag];
u_right_corner = [bump_len+bump_index(1)*nom_steplen*coef; bump_mag];
l_right_corner = [bump_len+bump_index(1)*nom_steplen*coef; 0];
upOrDowFlag = 0;
if length(bump_index) == 1
upOrDowFlag = 1;
h1 = line([l_left_corner(1) u_left_corner(1)], [l_left_corner(2) u_left_corner(2)], 'LineWidth',LW,'Color','Black');
h2 = line([u_left_corner(1) u_right_corner(1)],[u_left_corner(2) u_right_corner(2)], 'LineWidth',LW,'Color','Black');
h3 = line([l_right_corner(1) xlimTemp(2)], [u_left_corner(2) u_right_corner(2)],'Color','Black', 'LineWidth',LW);
else
h1 = line([l_left_corner(1) u_left_corner(1)], [l_left_corner(2) u_left_corner(2)], 'LineWidth',LW,'Color','Black');
h2 = line([u_left_corner(1) u_right_corner(1)],[u_left_corner(2) u_right_corner(2)], 'LineWidth',LW,'Color','Black');
h3 = line([u_right_corner(1) l_right_corner(1)],[u_right_corner(2) l_right_corner(2)], 'LineWidth',LW,'Color','Black');
h4 = line([l_right_corner(1) xlimTemp(2)], [0 0],'Color','Black', 'LineWidth',LW);
end
% area([l_left_corner(1) l_right_corner(1) ],[bump_mag bump_mag])
% h = drawmodel(w, xs(1,:), [0;0]);
shiftx = 0; shifty = 0;
vidObj = VideoWriter('vid_XX.avi');
% vidObj.FrameRate = 60;
open(vidObj);
nlegs = floor(pi / (2*alpha)); % roughly the right number of legs
hnlegs = floor(nlegs / 2); % half the number of legs
pause(5)
for i = 1:xlen
if i >= stepindices(bump_index(1))+1 && upOrDowFlag
shifty = bump_mag;
elseif i >= stepindices(bump_index(1))+1 && i < stepindices(bump_index(2))+1
shifty = bump_mag;
else
shifty = 0;
end
x = xs(i,:);
% drawnow;
drawmodel(w, x, [shiftx; shifty], h);
% q1 = x(1);
% footposn = [shiftx; shifty];
% hubx = -sin(q1) + footposn(1); huby = cos(q1) + footposn(2);
% angles = (-hnlegs:hnlegs) *2*alpha;
% legsx = [sin(-q1 + angles); sin(-q1 + angles + pi)] + hubx;
% legsy = [cos(-q1 + angles); cos(-q1 + angles + pi)] + huby;
% htemp = plot(legsx, legsy,'LineWidth',1,'EraseMode','background','Color','Blue'); %original
%
% h1 = plot([l_left_corner(1) u_left_corner(1)], [l_left_corner(2) u_left_corner(2)], 'LineWidth',LW,'Color','Black');
% h2 = plot([u_left_corner(1) u_right_corner(1)],[u_left_corner(2) u_right_corner(2)], 'LineWidth',LW,'Color','Black');
% h3 = plot([u_right_corner(1) l_right_corner(1)],[u_right_corner(2) l_right_corner(2)], 'LineWidth',LW,'Color','Black');
% h4 = plot([l_right_corner(1) xlimTemp(2)], [0 0],'Color','Black', 'LineWidth',LW);
% pause(0.02);
%
% delete(htemp); %delete(h1); delete(h2); delete(h3); delete(h4);
% clf;
% pause(0.02)
% delete(htemp)
drawnow;
M(i) = getframe(gcf);
writeVideo(vidObj,M(i))
if find(i == stepindices)
% shiftx = shiftx + 2*sin(alpha);
shiftx = shiftx + sin(abs(xs(i,1))) + sin(2*alpha + xs(i,1));
end
end
