%Starting point - STEP 01
function[counter, M] = b709(fivepts,counter)

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

% Motion data
t = [0:0.05:1]';                % Time data

for k = 1:7; %Position Data
    eval(['r1' num2str(k) '= [-5 + 0*t , 0*t, k-1 + 0*t]']);
    eval(['r2' num2str(k) '= [-5 + 0*t , 0*t, k-1 - 7*1 + 0*t]']);
    eval(['r3' num2str(k) '= [-5 + 0*t , 0*t, k-1 - 7*2 + 0*t]']);
    eval(['r4' num2str(k) '= [-5 + 0*t , 0*t, k-1 - 7*3 + 0*t]']);
    eval(['r5' num2str(k) '= [4 - 9*t , 0*t, k-1 - 7*4 + 0*t]']); 
    eval(['r6' num2str(k) '= [5 + 0*t , 0*t, k-1 + 0*t]']);
    eval(['r7' num2str(k) '= [6 + 0*t , 0*t, k-1 + 0*t]']); 
end

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,:));
    for k = 1:7;
        eval(['VertexData1' num2str(k) '(:,:,i_time) = GeoVerMakeBlock(r1' num2str(k) '(i_time,:),R,[Lx,Ly,Lz]);']);
        eval(['VertexData2' num2str(k) '(:,:,i_time) = GeoVerMakeBlock(r2' num2str(k) '(i_time,:),R,[Lx,Ly,Lz]);']);
        eval(['VertexData3' num2str(k) '(:,:,i_time) = GeoVerMakeBlock(r3' num2str(k) '(i_time,:),R,[Lx,Ly,Lz]);']);
        eval(['VertexData4' num2str(k) '(:,:,i_time) = GeoVerMakeBlock(r4' num2str(k) '(i_time,:),R,[Lx,Ly,Lz]);']);
        eval(['VertexData5' num2str(k) '(:,:,i_time) = GeoVerMakeBlock(r5' num2str(k) '(i_time,:),R,[Lx,Ly,Lz]);']);
        eval(['VertexData6' num2str(k) '(:,:,i_time) = GeoVerMakeBlock(r6' num2str(k) '(i_time,:),R,[Lx,Ly,Lz]);']);
        eval(['VertexData7' num2str(k) '(:,:,i_time) = GeoVerMakeBlock(r7' num2str(k) '(i_time,:),R,[Lx,Ly,Lz]);']);
    end
    
    for k = 1:7;
        eval(['[X1' num2str(k) ', Y1' num2str(k) ', Z1' num2str(k) '] = GeoPatMakeBlock(VertexData1' num2str(k) '(:,:,i_time))']);
        eval(['[X2' num2str(k) ', Y2' num2str(k) ', Z2' num2str(k) '] = GeoPatMakeBlock(VertexData2' num2str(k) '(:,:,i_time))']);
        eval(['[X3' num2str(k) ', Y3' num2str(k) ', Z3' num2str(k) '] = GeoPatMakeBlock(VertexData3' num2str(k) '(:,:,i_time))']);
        eval(['[X4' num2str(k) ', Y4' num2str(k) ', Z4' num2str(k) '] = GeoPatMakeBlock(VertexData4' num2str(k) '(:,:,i_time))']);
        eval(['[X5' num2str(k) ', Y5' num2str(k) ', Z5' num2str(k) '] = GeoPatMakeBlock(VertexData5' num2str(k) '(:,:,i_time))']);
        eval(['[X6' num2str(k) ', Y6' num2str(k) ', Z6' num2str(k) '] = GeoPatMakeBlock(VertexData6' num2str(k) '(:,:,i_time))']);
        eval(['[X7' num2str(k) ', Y7' num2str(k) ', Z7' num2str(k) '] = GeoPatMakeBlock(VertexData7' num2str(k) '(:,:,i_time))']);
    end
    
    for k = 1:7;
        eval(['PatchData_X1' num2str(k) '(:,:,i_time) = X1' num2str(k) '']);
        eval(['PatchData_Y1' num2str(k) '(:,:,i_time) = Y1' num2str(k) '']);
        eval(['PatchData_Z1' num2str(k) '(:,:,i_time) = Z1' num2str(k) '']);
        eval(['PatchData_X2' num2str(k) '(:,:,i_time) = X2' num2str(k) '']);
        eval(['PatchData_Y2' num2str(k) '(:,:,i_time) = Y2' num2str(k) '']);
        eval(['PatchData_Z2' num2str(k) '(:,:,i_time) = Z2' num2str(k) '']);
        eval(['PatchData_X3' num2str(k) '(:,:,i_time) = X3' num2str(k) '']);
        eval(['PatchData_Y3' num2str(k) '(:,:,i_time) = Y3' num2str(k) '']);
        eval(['PatchData_Z3' num2str(k) '(:,:,i_time) = Z3' num2str(k) '']);
        eval(['PatchData_X4' num2str(k) '(:,:,i_time) = X4' num2str(k) '']);
        eval(['PatchData_Y4' num2str(k) '(:,:,i_time) = Y4' num2str(k) '']);
        eval(['PatchData_Z4' num2str(k) '(:,:,i_time) = Z4' num2str(k) '']);
        eval(['PatchData_X5' num2str(k) '(:,:,i_time) = X5' num2str(k) '']);
        eval(['PatchData_Y5' num2str(k) '(:,:,i_time) = Y5' num2str(k) '']);
        eval(['PatchData_Z5' num2str(k) '(:,:,i_time) = Z5' num2str(k) '']);
        eval(['PatchData_X6' num2str(k) '(:,:,i_time) = X6' num2str(k) '']);
        eval(['PatchData_Y6' num2str(k) '(:,:,i_time) = Y6' num2str(k) '']);
        eval(['PatchData_Z6' num2str(k) '(:,:,i_time) = Z6' num2str(k) '']);
        eval(['PatchData_X7' num2str(k) '(:,:,i_time) = X7' num2str(k) '']);
        eval(['PatchData_Y7' num2str(k) '(:,:,i_time) = Y7' num2str(k) '']);
        eval(['PatchData_Z7' num2str(k) '(:,:,i_time) = Z7' num2str(k) '']);
    end 
end

% Draw initial figure
for k = 1:7;
    eval(['h1' num2str(k) ' = patch(PatchData_X1' num2str(k) '(:,:,1),PatchData_Y1' num2str(k) '(:,:,1),PatchData_Z1' num2str(k) '(:,:,1))']); 
    c = eval(['h1' num2str(k)]); 
    set(c,'FaceLighting','phong','EdgeLighting','phong');
    set(c,'EraseMode','normal');
    set(c,'facecolor','w');
    
    eval(['h2' num2str(k) ' = patch(PatchData_X2' num2str(k) '(:,:,1),PatchData_Y2' num2str(k) '(:,:,1),PatchData_Z2' num2str(k) '(:,:,1))']);  
    c = eval(['h2' num2str(k)]); 
    set(c,'FaceLighting','phong','EdgeLighting','phong');
    set(c,'EraseMode','normal');
    set(c,'facecolor','w');
    
    eval(['h3' num2str(k) ' = patch(PatchData_X3' num2str(k) '(:,:,1),PatchData_Y3' num2str(k) '(:,:,1),PatchData_Z3' num2str(k) '(:,:,1))']);  
    c = eval(['h3' num2str(k)]); 
    set(c,'FaceLighting','phong','EdgeLighting','phong');
    set(c,'EraseMode','normal');
    set(c,'facecolor','w');
    
    eval(['h4' num2str(k) ' = patch(PatchData_X4' num2str(k) '(:,:,1),PatchData_Y4' num2str(k) '(:,:,1),PatchData_Z4' num2str(k) '(:,:,1))']);  
    c = eval(['h4' num2str(k)]); 
    set(c,'FaceLighting','phong','EdgeLighting','phong');
    set(c,'EraseMode','normal');
    set(c,'facecolor','w');
    
    eval(['h5' num2str(k) ' = patch(PatchData_X5' num2str(k) '(:,:,1),PatchData_Y5' num2str(k) '(:,:,1),PatchData_Z5' num2str(k) '(:,:,1))']);  
    c = eval(['h5' num2str(k)]); 
    set(c,'FaceLighting','phong','EdgeLighting','phong');
    set(c,'EraseMode','normal');
    set(c,'facecolor','w');
    
    eval(['h6' num2str(k) ' = patch(PatchData_X6' num2str(k) '(:,:,1),PatchData_Y6' num2str(k) '(:,:,1),PatchData_Z6' num2str(k) '(:,:,1))']);  
    c = eval(['h6' num2str(k)]); 
    set(c,'FaceLighting','phong','EdgeLighting','phong');
    set(c,'EraseMode','normal');
    set(c,'facecolor','w');
    
    eval(['h7' num2str(k) ' = patch(PatchData_X7' num2str(k) '(:,:,1),PatchData_Y7' num2str(k) '(:,:,1),PatchData_Z7' num2str(k) '(:,:,1))']);  
    c = eval(['h7' num2str(k)]); 
    set(c,'FaceLighting','phong','EdgeLighting','phong');
    set(c,'EraseMode','normal');
    set(c,'facecolor','w');
    
end

if fivepts == 1
    set(h24,'facecolor','red');
    set(h35,'facecolor','red');
    set(h44,'facecolor','red');
    set(h33,'facecolor','red');
    set(h34,'facecolor','black');
end

% Animation Loop
for i_time=1:n_time
    
    for k = 1:7;
        c = eval(['h1' num2str(k)]); 
        set(c,'XData',eval(['PatchData_X1'  num2str(k) '(:,:,i_time)']));
        set(c,'YData',eval(['PatchData_Y1'  num2str(k) '(:,:,i_time)']));
        set(c,'ZData',eval(['PatchData_Z1'  num2str(k) '(:,:,i_time)']));
        
        c = eval(['h2' num2str(k)]); 
        set(c,'XData',eval(['PatchData_X2'  num2str(k) '(:,:,i_time)']));
        set(c,'YData',eval(['PatchData_Y2'  num2str(k) '(:,:,i_time)']));
        set(c,'ZData',eval(['PatchData_Z2'  num2str(k) '(:,:,i_time)']));
        
        c = eval(['h3' num2str(k)]); 
        set(c,'XData',eval(['PatchData_X3'  num2str(k) '(:,:,i_time)']));
        set(c,'YData',eval(['PatchData_Y3'  num2str(k) '(:,:,i_time)']));
        set(c,'ZData',eval(['PatchData_Z3'  num2str(k) '(:,:,i_time)']));
        
        c = eval(['h4' num2str(k)]); 
        set(c,'XData',eval(['PatchData_X4'  num2str(k) '(:,:,i_time)']));
        set(c,'YData',eval(['PatchData_Y4'  num2str(k) '(:,:,i_time)']));
        set(c,'ZData',eval(['PatchData_Z4'  num2str(k) '(:,:,i_time)']));
        
        c = eval(['h5' num2str(k)]); 
        set(c,'XData',eval(['PatchData_X5'  num2str(k) '(:,:,i_time)']));
        set(c,'YData',eval(['PatchData_Y5'  num2str(k) '(:,:,i_time)']));
        set(c,'ZData',eval(['PatchData_Z5'  num2str(k) '(:,:,i_time)'])); 
        
        c = eval(['h6' num2str(k)]); 
        set(c,'XData',eval(['PatchData_X6'  num2str(k) '(:,:,i_time)']));
        set(c,'YData',eval(['PatchData_Y6'  num2str(k) '(:,:,i_time)']));
        set(c,'ZData',eval(['PatchData_Z6'  num2str(k) '(:,:,i_time)'])); 
        
        c = eval(['h7' num2str(k)]); 
        set(c,'XData',eval(['PatchData_X7'  num2str(k) '(:,:,i_time)']));
        set(c,'YData',eval(['PatchData_Y7'  num2str(k) '(:,:,i_time)']));
        set(c,'ZData',eval(['PatchData_Z7'  num2str(k) '(:,:,i_time)'])); 
    end
    
    M(counter) = getframe(gcf);
    counter = counter + 1;  
    
end

