% carplot trailer...animate van with trailer backing up...
k = 1;
clf
axis equal
axis([-10 110 -45 20])
axis manual
% hold on
w = 2;
L = 3.949;

Lt = 9.172;
wt = 2.4459;
% plot obstacles...


xa = simout(:,1);
ya = simout(:,2);

xb = simout(:,3);
yb = simout(:,4);

xc = simout(:,5);
yc = simout(:,6);

theta = simout(:,7);
% psi is in workspace
% 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);

% trailer coords...
txrr = xc + wt/2*cos(theta - pi/2);
tyrr = yc + wt/2*sin(theta - pi/2);
txlr = xc + wt/2*cos(theta + pi/2);
tylr = yc + wt/2*sin(theta + pi/2);

txrf = txrr - 7*Lt/8*cos(theta);
tyrf = tyrr - 7*Lt/8*sin(theta);
txlf = txlr - 7*Lt/8*cos(theta);
tylf = tylr - 7*Lt/8*sin(theta);

cg = [(xa + xb)/2, (ya + yb)/2];
% u = 0:0.2:100;
u = [t, t+30];

while k <=length(simout)
    clf
    yref = (u-13.121)*sin(pi/6).*(u>13.121).*(u<40) + (u>=40).*(2*13.439  - (u-13.121)*sin(pi/6));
plot(u,yref,'k--',u,yref+7,'k-',u,yref-7,'k-')
    axis equal
% axis([-10 35 -5 10])

axis([-10 130 -45 20])
axis manual
hold on
    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([txrr(k) txrf(k) txlf(k) txlr(k) txrr(k)],[tyrr(k) tyrf(k) tylf(k) tylr(k) tyrr(k)],'b-')
 % now for the tongue:
 plot([xb(k) xc(k)], [yb(k) yc(k)],'b-','LineWidth',2)
%     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
