%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% G R A F Y %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Vykreslení polohy
hold on
% h2 = plot3([X2(:,1)], [X2(:,2)],[X2(:,3)], 'Linewidth', 2, 'Color', 'm');
% h3 = plot3([X3(:,1)], [X3(:,2)],[X3(:,3)], 'Linewidth', 2, 'Color', 'g');
% h4 = plot3([X4(:,1)], [X4(:,2)],[X4(:,3)], 'Linewidth', 2, 'Color', 'y');
% h5 = plot3([X5(:,1)], [X5(:,2)],[X5(:,3)], 'Linewidth', 2, 'Color', 'b');
h = plot3([Xtraj(:,1)], [Xtraj(:,2)],[Xtraj(:,3)], 'Linewidth', 2, 'Color', 'r');

% % Vykreslení bezkolizních bodů
% hb = plot3(bezkoliznibody(:,1), bezkoliznibody(:,2), bezkoliznibody(:,3), 'y*','markersize',5, 'linewidth',5);

% % Vykreslení bodů mezi
% hm = plot3(mezi(:,1), mezi(:,2), mezi(:,3), 'r*','markersize',5, 'linewidth',5);



%%%%%%%%%%%%%%%%%%%%%%%%%%
%% A N I M A C E %%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% 
% g = 8; % krok pro vyobrazení manipulátoru
% Lt = length(timepoints);
% j = 1;
% hold on
% % waitforbuttonpress
% for i = 1 : g-1
%     for kk = i*round(Lt/g)+1 - round(Lt/g) : i*round(Lt/g)
%         h = plot3(Xtraj(kk,1), Xtraj(kk,2), Xtraj(kk,3),'r.','markersize',1);
%         drawnow limitrate;
%         
% %         if j == 20
% %             saveas(gcf,['pos',num2str(kk)],'png')
% %             j = 0;
% %         end
% %         j = j+1;
%     end
%     grid on
%     SimulacniModel_robot_carkovane(Xtraj(kk,1), Xtraj(kk,2), Xtraj(kk,3),0, robot.sirka_y, robot.delka_x, robot.vyska_z, robot.delkaNuzek, robot.prisavkySirka, robot.prisavkyDelka);
% end
%  
% j = 1;
% for kk = i*round(Lt/g) + 1 : g*Lt/g
%     h = plot3(Xtraj(kk,1), Xtraj(kk,2), Xtraj(kk,3),'r.','markersize',1);
%     drawnow limitrate;
% %     if j == 20
% %         saveas(gcf,['pos',num2str(kk)],'png')
% %         j = 0;
% %     end
% %     j = j+1;
% end    
% SimulacniModel_robot(Xtraj(kk,1), Xtraj(kk,2), Xtraj(kk,3),0, robot.sirka_y, robot.delka_x, robot.vyska_z, robot.delkaNuzek, robot.prisavkySirka, robot.prisavkyDelka);
% % saveas(gcf,['pos',num2str(kk+1)],'png')



%%%%%%%%%%%%%%%%%%%%%
%% S U B P L O T Y %%
%%%%%%%%%%%%%%%%%%%%%
% hold off
% % OSA X
% figure
% 
% subplot(4,1,1)
% plot(timepoints,j(:,1))
% xlabel('time [s]')
% ylabel('j_x [m/s^3]')
% xlim([0 time])
% 
% subplot(4,1,2)
% plot(timepoints,a(:,1))
% xlabel('time [s]')
% ylabel('a_x [m/s^2]')
% xlim([0 time])
% 
% subplot(4,1,3)
% plot(timepoints,v(:,1))
% xlabel('time [s]')
% ylabel('v_x [m/s]')
% xlim([0 time])
% ylim([min(v(:,1))*1.3, max(v(:,1))*1.3])
% 
% subplot(4,1,4)
% plot(timepoints,Xtraj(:,1))
% xlabel('time [s]')
% ylabel('x [m]')
% xlim([0 time])
% ylim([min(Xtraj(:,1))*1.3, max(Xtraj(:,1))*1.3])
% 
% sgtitle('osa x')
% 
% 
% 
% 
% % OSA Y
% 
% figure
% 
% subplot(4,1,1)
% plot(timepoints,j(:,2))
% xlabel('time [s]')
% ylabel('j_y [m/s^3]')
% xlim([0 time])
% 
% 
% subplot(4,1,2)
% plot(timepoints,a(:,2))
% xlabel('time [s]')
% ylabel('a_y [m/s^2]')
% xlim([0 time])
% 
% subplot(4,1,3)
% plot(timepoints,v(:,2))
% xlabel('time [s]')
% ylabel('v_y [m/s]')
% xlim([0 time])
% ylim([min(v(:,2))*1.3, max(v(:,2))*1.3])
% 
% subplot(4,1,4)
% plot(timepoints,Xtraj(:,2))
% xlabel('time [s]')
% ylabel('y [m]')
% xlim([0 time])
% ylim([min(Xtraj(:,2))*1.3, max(Xtraj(:,2))*1.3])
% 
% sgtitle('osa y')
% 
% 
% % OSA Z
% 
% figure
% 
% subplot(4,1,1)
% plot(timepoints,j(:,3))
% xlabel('time [s]')
% ylabel('j_z [m/s^3]')
% xlim([0 time])
% 
% subplot(4,1,2)
% plot(timepoints,a(:,3))
% xlabel('time [s]')
% ylabel('a_z [m/s^2]')
% xlim([0 time])
% 
% subplot(4,1,3)
% plot(timepoints,v(:,3))
% xlabel('time [s]')
% ylabel('v_z [m/s]')
% xlim([0 time])
% ylim([min(v(:,3))*1.3, max(v(:,3))*1.3])
% 
% subplot(4,1,4)
% plot(timepoints,Xtraj(:,3))
% xlabel('time [s]')
% ylabel('z [m]')
% xlim([0 time])
% ylim([min(Xtraj(:,3))*1.3, max(Xtraj(:,3))*1.3])
% 
% sgtitle('osa z')
% 
% 
% % ABSOLUT
% 
% figure
% 
% subplot(3,1,1)
% plot(timepoints,j_abs)
% xlabel('time [s]')
% ylabel('j [m/s^3]')
% xlim([0 time])
% 
% subplot(3,1,2)
% plot(timepoints,a_abs)
% xlabel('time [s]')
% ylabel('a [m/s^2]')
% xlim([0 time])
% 
% subplot(3,1,3)
% plot(timepoints,v_abs)
% xlabel('time [s]')
% ylabel('v [m/s]')
% xlim([0 time])
% ylim([min(v_abs)*1.3, max(v_abs)*1.3])
% 
% sgtitle('Absolutní hodnoty veličin')