% plotting script for car data;
k = 1;
axis([0 105 -11 11])
axis manual
axis equal
hold on

w = 1.5720;
L = 3.0293;
% variable to keep only every 10th entry in output arrays
% keep = 1:10:length(simout);
x1 = [0 105];
y1 = [-10 -10];
plot(x1,y1,'k-')
hold on
y2 = zeros(1,2);
plot(x1,y2,'k--')

y3 = -y1;
plot(x1,y3,'k-')

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
