% plotting script for car data;
% modify for parallel parking...
k = 1;

axis equal
axis([-10 10 -8 3])

hold on

w = 1.5720;
L = 3.0293;
% plot obstacles...

x1 = [-10 -10 -3 -3 -10];
y1 = [-2 2 2 -2 -2];

patch(x1,y1,'c')
hold on
patch(-x1,y1,'c')
plot([-6 6],[2 2],'c-','LineWidth',2)

xa = simout(:,1);
xb = simout(:,3);
ya = simout(:,2);
yb = simout(:,4);

% psi = psi(keep);

xrr = xa + w/2*cos(psi - pi/2);
yrr = ya + w/2*sin(psi - pi/2);
xrf = xb + w/2*cos(psi - pi/2);
yrf = yb + w/2*sin(psi - pi/2);
xlf = xb + w/2*cos(psi + pi/2);
ylf = yb + w/2*sin(psi + pi/2);
xlr = xa + w/2*cos(psi + pi/2);
ylr = ya + w/2*sin(psi + pi/2);

cg = [(xa + xb)/2, (ya + yb)/2];

while k <=length(simout)
    plot(cg(k,1),cg(k,2),'b.')
    plot([xrr(k) xrf(k) xlf(k) xlr(k) xrr(k)],[yrr(k) yrf(k) ylf(k) ylr(k) yrr(k)],'b-')
%     plot([simout(k,1) simout(k,3)],...% simout(k,5) simout(k,7) simout(k,1)],...
%         [simout(k,2) simout(k,4)],'b-')% simout(k,6) simout(k,8), simout(k,2)],'b-')
M(:,k) = getframe;

k = k + 1;
end