%Starting point - STEP 02

clearvars -except vo ; %clear all except video object vo;

% Block specification
Lx = 1;
Ly = 1;
Lz = 1;

% Motion data
t = [0:0.01:0.2]';                % Time data

r1 = [ -7 - 0*t, 0*t, 1 + 0*t]; % Position data
r3 = [ -6 - 0*t, 0*t, 1 + 0*t]; % Position data
r5 = [ -5 - 0*t, 0*t, 1 + 0*t]; % Position data
r7 = [ -4 - 0*t, 0*t, 1 + 0*t]; % Position data

r2 = [  3 + 0*t, 0*t, 1 + 0*t]; % Position data
r4 = [  4 + 0*t, 0*t, 1 + 0*t]; % Position data
r6 = [  5 + 0*t, 0*t, 1 + 0*t]; % Position data
r8 = [  6 + 0*t, 0*t, 1 + 0*t]; % Position data

A = [0*t, 0*t, 0*t];             % Orientation data (x-y-z Euler angle)

n_time = length(t);

% Compute propagation of vertices and patches
for i_time=1:n_time
    
    R = Euler2R(A(i_time,:));
    VertexData1(:,:,i_time) = GeoVerMakeBlock(r1(i_time,:),R,[Lx,Ly,Lz]);
    VertexData2(:,:,i_time) = GeoVerMakeBlock(r2(i_time,:),R,[Lx,Ly,Lz]);
    VertexData3(:,:,i_time) = GeoVerMakeBlock(r3(i_time,:),R,[Lx,Ly,Lz]);
    VertexData4(:,:,i_time) = GeoVerMakeBlock(r4(i_time,:),R,[Lx,Ly,Lz]);
    VertexData5(:,:,i_time) = GeoVerMakeBlock(r5(i_time,:),R,[Lx,Ly,Lz]);
    VertexData6(:,:,i_time) = GeoVerMakeBlock(r6(i_time,:),R,[Lx,Ly,Lz]);
    VertexData7(:,:,i_time) = GeoVerMakeBlock(r7(i_time,:),R,[Lx,Ly,Lz]);
    VertexData8(:,:,i_time) = GeoVerMakeBlock(r8(i_time,:),R,[Lx,Ly,Lz]);
    [X1,Y1,Z1] = GeoPatMakeBlock(VertexData1(:,:,i_time));
    [X2,Y2,Z2] = GeoPatMakeBlock(VertexData2(:,:,i_time));
    [X3,Y3,Z3] = GeoPatMakeBlock(VertexData3(:,:,i_time));
    [X4,Y4,Z4] = GeoPatMakeBlock(VertexData4(:,:,i_time));
    [X5,Y5,Z5] = GeoPatMakeBlock(VertexData5(:,:,i_time));
    [X6,Y6,Z6] = GeoPatMakeBlock(VertexData6(:,:,i_time));
    [X7,Y7,Z7] = GeoPatMakeBlock(VertexData7(:,:,i_time));
    [X8,Y8,Z8] = GeoPatMakeBlock(VertexData8(:,:,i_time));
    PatchData_X1(:,:,i_time) = X1;
    PatchData_Y1(:,:,i_time) = Y1;
    PatchData_Z1(:,:,i_time) = Z1;
    PatchData_X2(:,:,i_time) = X2;
    PatchData_Y2(:,:,i_time) = Y2;
    PatchData_Z2(:,:,i_time) = Z2;
    PatchData_X3(:,:,i_time) = X3;
    PatchData_Y3(:,:,i_time) = Y3;
    PatchData_Z3(:,:,i_time) = Z3;
    PatchData_X4(:,:,i_time) = X4;
    PatchData_Y4(:,:,i_time) = Y4;
    PatchData_Z4(:,:,i_time) = Z4;
    PatchData_X5(:,:,i_time) = X5;
    PatchData_Y5(:,:,i_time) = Y5;
    PatchData_Z5(:,:,i_time) = Z5;
    PatchData_X6(:,:,i_time) = X6;
    PatchData_Y6(:,:,i_time) = Y6;
    PatchData_Z6(:,:,i_time) = Z6;
    PatchData_X7(:,:,i_time) = X7;
    PatchData_Y7(:,:,i_time) = Y7;
    PatchData_Z7(:,:,i_time) = Z7;
    PatchData_X8(:,:,i_time) = X8;
    PatchData_Y8(:,:,i_time) = Y8;
    PatchData_Z8(:,:,i_time) = Z8;
end

% Draw initial figure
h1 = patch(PatchData_X1(:,:,1),PatchData_Y1(:,:,1),PatchData_Z1(:,:,1),'r');
set(h1,'FaceLighting','phong','EdgeLighting','phong');
set(h1,'EraseMode','normal');
% Draw initial figure
h2 = patch(PatchData_X2(:,:,1),PatchData_Y2(:,:,1),PatchData_Z2(:,:,1),'r');
set(h2,'FaceLighting','phong','EdgeLighting','phong');
set(h2,'EraseMode','normal');
% Draw initial figure
h3 = patch(PatchData_X3(:,:,1),PatchData_Y3(:,:,1),PatchData_Z3(:,:,1),'k');
set(h3,'FaceLighting','phong','EdgeLighting','phong');
set(h3,'EraseMode','normal');
% Draw initial figure
h4 = patch(PatchData_X4(:,:,1),PatchData_Y4(:,:,1),PatchData_Z4(:,:,1),'k');
set(h4,'FaceLighting','phong','EdgeLighting','phong');
set(h4,'EraseMode','normal');
% Draw initial figure
h5 = patch(PatchData_X5(:,:,1),PatchData_Y5(:,:,1),PatchData_Z5(:,:,1),'r');
set(h5,'FaceLighting','phong','EdgeLighting','phong');
set(h5,'EraseMode','normal');
% Draw initial figure
h6 = patch(PatchData_X6(:,:,1),PatchData_Y6(:,:,1),PatchData_Z6(:,:,1),'r');
set(h6,'FaceLighting','phong','EdgeLighting','phong');
set(h6,'EraseMode','normal');
% Draw initial figure
h7 = patch(PatchData_X7(:,:,1),PatchData_Y7(:,:,1),PatchData_Z7(:,:,1),'k');
set(h7,'FaceLighting','phong','EdgeLighting','phong');
set(h7,'EraseMode','normal');
% Draw initial figure
h8 = patch(PatchData_X8(:,:,1),PatchData_Y8(:,:,1),PatchData_Z8(:,:,1),'k');
set(h8,'FaceLighting','phong','EdgeLighting','phong');
set(h8,'EraseMode','normal');

% Axes settings
xlabel('x','FontSize',14);
ylabel('y','FontSize',14);
zlabel('z','FontSize',14);
set(gca,'FontSize',14);
axis vis3d equal;
view([0,0]);
camlight;
grid on;
xlim([-10,10]);
ylim([-2,8]);
zlim([-0.5,4]);


%Set texts
text1 = text(-6.59,0,1.5,'0', 'color','w');
text3 = text(-5.59,0,1.5,'2', 'color','w');
text5 = text(-4.59,0,1.5,'4', 'color','w');
text7 = text(-3.59,0,1.5,'6', 'color','w');

text2 = text( 3.41,0,1.5,'1', 'color','w');
text4 = text( 4.41,0,1.5,'3', 'color','w');
text6 = text( 5.41,0,1.5,'5', 'color','w');
text8 = text( 6.41,0,1.5,'7', 'color','w');

% Animation Loop
for i_time=1:n_time
    
    set(h1,'XData',PatchData_X1(:,:,i_time));
    set(h1,'YData',PatchData_Y1(:,:,i_time));
    set(h1,'ZData',PatchData_Z1(:,:,i_time));
    set(text1,'position',[-6.59 - 0*t(i_time),0*t(i_time),1.5 + 0*t(i_time)]);
    
    set(h3,'XData',PatchData_X3(:,:,i_time));
    set(h3,'YData',PatchData_Y3(:,:,i_time));
    set(h3,'ZData',PatchData_Z3(:,:,i_time));
    set(text3,'position',[-5.59 - 0*t(i_time),0*t(i_time),1.5 + 0*t(i_time)]);
    
    set(h5,'XData',PatchData_X5(:,:,i_time));
    set(h5,'YData',PatchData_Y5(:,:,i_time));
    set(h5,'ZData',PatchData_Z5(:,:,i_time));
    set(text5,'position',[-4.59 - 0*t(i_time),0*t(i_time),1.5 + 0*t(i_time)]);
    
    set(h7,'XData',PatchData_X7(:,:,i_time));
    set(h7,'YData',PatchData_Y7(:,:,i_time));
    set(h7,'ZData',PatchData_Z7(:,:,i_time));
    set(text7,'position',[-3.59 - 0*t(i_time),0*t(i_time),1.5 + 0*t(i_time)]);
    
    %
        
    set(h2,'XData',PatchData_X2(:,:,i_time));
    set(h2,'YData',PatchData_Y2(:,:,i_time));
    set(h2,'ZData',PatchData_Z2(:,:,i_time));
    set(text2,'position',[ 3.41 + 0*t(i_time),0*t(i_time),1.5 + 0*t(i_time)]);
    
    set(h4,'XData',PatchData_X4(:,:,i_time));
    set(h4,'YData',PatchData_Y4(:,:,i_time));
    set(h4,'ZData',PatchData_Z4(:,:,i_time));
    set(text4,'position',[ 4.41 + 0*t(i_time),0*t(i_time),1.5 + 0*t(i_time)]);
        
    set(h6,'XData',PatchData_X6(:,:,i_time));
    set(h6,'YData',PatchData_Y6(:,:,i_time));
    set(h6,'ZData',PatchData_Z6(:,:,i_time));
    set(text6,'position',[ 5.41 + 0*t(i_time),0*t(i_time),1.5 + 0*t(i_time)]);
    
    set(h8,'XData',PatchData_X8(:,:,i_time));
    set(h8,'YData',PatchData_Y8(:,:,i_time));
    set(h8,'ZData',PatchData_Z8(:,:,i_time));
    set(text8,'position',[ 6.41 + 0*t(i_time),0*t(i_time),1.5 + 0*t(i_time)]);
    
    cF=getframe(gcf); % current frame
    writeVideo(vo,cF);
end

