% -------------------------------------------------------------------------------------------------------------------------------------------
% @Copyright:  M. Bollhoefer and V. Mehrmann
%	       Numerische Mathematik: Eine Projektorientierte Einfuehrung Fuer Ingenieure, Mathematiker und Naturwissenschaftler,Springer, 2004.
% -------------------------------------------------------------------------------------------------------------------------------------------

% ----------------------------------------------------------------------------------------------
% Visualization of the truck driving over the street defined as s = STREET(t)
% plotcabin(t,x)
% plotcabin(t,x,s)
% Input: t - vector with time points t_0,t_1,...,t_n, length n+1
%	 x - 6 x (n+1) with numerical approximations of x for time points t_0,t_1,...,t_n
%	 s - street, if it is not given it will be defined as s = STREET(t)
% ----------------------------------------------------------------------------------------------
function plotcabin(t,x,s)

t0=t(1);
te=t(end);

% Displacement along y-axis
y(1:3,1)=[1.5;...
          1.0;...
	  0.3];

       
if nargin==2
   for i=1:length(t)
       s(i)=STREET(t(i));
   end
end

mn=-1;
mn=min(mn,y(1)+min(x(1,:)));
mn=min(mn,y(2)+min(x(2,:)));
mn=min(mn,y(3)+min(x(3,:)));
mx=0;
mx=max(mx,y(1)+max(x(1,:)));
mx=max(mx,y(2)+max(x(2,:)));
mx=max(mx,y(3)+max(x(3,:)));
       


deltat=(te-t0)/100;
tt=t0;
for i=1:length(t)
    if t(i)>=tt
       x1=min(x(1,i),3); x1=max(x1,-2);
       x2=min(x(2,i),3); x2=max(x2,-2);
       x3=min(x(3,i),3); x3=max(x3,-2);
       if nargin==2
	  plotcabin1(t(i),y(1)+x1,y(2)+x2,y(3)+x3);
       else
	  dt =t(find(t(i)-1.1<=t & t<=t(i)+1.0));
	  sdt=s(find(t(i)-1.1<=t & t<=t(i)+1.0));
	  plotcabin1(t(i),y(1)+x1,y(2)+x2,y(3)+x3,dt,sdt);
       end
		  
       axis equal
       axis([-1.5 1.5 mn-0.2*(mx-mn) mx+0.2*(mx-mn)+1.5]);
       xlabel(t(i))
       tt=tt+deltat;
       pause(0.02);
    end
end 


end

function plotcabin1(t,u1,u2,u3,dt,sdt)


if nargin==4
   s=STREET(t);
else
   s=sdt(find(dt==t));
end

% Tire
r=abs(s-u3);
phi=0:.1:2*pi;
c=r*cos(phi);
s=u3+r*sin(phi);
plot(c,s,'-b')
hold on

% Damper
u=u3+r;
o=u2-.1;
plot([0,0],[u,o],'-k');
u=u+.05;
o=o-.05;
dy=(o-u)/4;
for y=u:dy:o-dy
    plot([-0.1,0.1],[y,y],'-k');
    plot([-0.1,0.1],[y,y+dy],'-k');
end 
y=o;
plot([-0.1,0.1],[y,y],'-k');

% Cabin
plot([-0.5 0.5],[u2+.1,u2+.1],'-m')
plot([-0.5 0.5],[u2-.1,u2-.1],'-m')
plot([0.5 0.5],[u2-.1,u2+.1],'-m')
plot([-0.5 -0.5],[u2-.1,u2+.1],'-m')

plot([0.5 0.5],[u2+.1,u2+.5],'-k')
plot([-0.5 -0.5],[u2+.1,u2+1.5],'-k')
plot([-0.5  0.2],[u2+1.5,u2+1.5],'-k')
plot([0.2  0.5],[u2+1.5,u2+.5],'-k')

% Loading platform 
plot([-1.1 -0.6],[u2-.1,u2-.1],':k')
plot([-0.6 -0.6],[u2-.1,u2+.5],':k')
plot([-1.1 -0.6],[u2+.5,u2+.5],':k')
plot([-0.6 -0.6],[u2+.5,u2+2.0],':k')
plot([-1.1 -0.6],[u2+2.0,u2+2.0],':k')


% Damper
u=u2+.1;
o=u1-.075;
plot([0,0],[u,o],'-k');
u=u+.05;
o=o-.05;
dy=(o-u)/4;
for y=u:dy:o-dy
    plot([-0.1,0.1],[y,y],'-k');
    plot([-0.1,0.1],[y,y+dy],'-k');
end 
y=o;
plot([-0.1,0.1],[y,y],'-k');


% Seat
plot([-0.3 0.2],[u1-.075,u1-.075],'-g')
plot([-0.3 -0.4],[u1-.075,u1+.7],'-k')
plot([-0.4 -0.3],[u1+.7,u1+.7],'-k')
plot([-0.3 -0.2],[u1+.7,u1+.075],'-k')
plot([-0.2  0.2],[u1+.075,u1+.075],'-g')
plot([ 0.2  0.2],[u1+.075,u1-.075],'-g')


% Street
if nargin==4
   dt=(t-1.1):.1:(t+1.0);
   for i=1:length(dt)
       sdt(i)=STREET(dt(i));
   end
end
plot(dt-t,sdt,'-k');
hold off

v=axis;
dx=v(2)-v(1);
v(1)=v(1)-.2*dx;
v(2)=v(2)+.2*dx;
dy=v(4)-v(3);
v(3)=v(3)-.2*dy;
v(4)=v(4)+.2*dy;
axis(v)
axis equal
end
