plot3(p_i(1), p_i(2), p_i(3), 'k*', 'MarkerSize', 16)
text(p_i(1) + 0.05, p_i(2), p_i(3)+0.05, '$p_i$', 'Interpreter', 'latex', 'FontSize', 16)
plot3(p_int(1), p_int(2), p_int(3),'k*', 'MarkerSize', 16)
text(p_int(1), p_int(2), p_int(3)+0.05, '$p_{int}$', 'Interpreter', 'latex', 'FontSize', 16)
plot3(p_f(1), p_f(2), p_f(3),'k*', 'MarkerSize', 16)
text(p_f(1) + 0.05, p_f(2), p_f(3)+0.05, '$p_f$', 'Interpreter', 'latex', 'FontSize', 16)
xlabel('x [m]','Interpreter','latex')
ylabel('y [m]','Interpreter','latex')
zlabel('z [m]','Interpreter','latex')
title("SENZA FUNZIONALE",'Interpreter','latex')
plot3(p_i(1), p_i(2), p_i(3), 'k*', 'MarkerSize', 16)
text(p_i(1) + 0.05, p_i(2), p_i(3)+0.05, '$p_i$', 'Interpreter', 'latex', 'FontSize', 16)
plot3(p_int(1), p_int(2), p_int(3),'k*', 'MarkerSize', 16)
text(p_int(1), p_int(2), p_int(3)+0.05, '$p_{int}$', 'Interpreter', 'latex', 'FontSize', 16)
plot3(p_f(1), p_f(2), p_f(3),'k*', 'MarkerSize', 16)
text(p_f(1) + 0.05, p_f(2), p_f(3)+0.05, '$p_f$', 'Interpreter', 'latex', 'FontSize', 16)
xlabel('x [m]','Interpreter','latex')
ylabel('y [m]','Interpreter','latex')
ylabel('z [m]','Interpreter','latex')
title("CON FUNZIONALE",'Interpreter','latex')
function Simulazione(XY1, XY2, XY3, XYe, XYd, XY1_man, XY2_man, XY3_man, XYe_man, t, p_i, p_int, p_f, W, W_fun, lmax)
sgt = sgtitle('4 DoF Manipulator','Color','black');
sgt.Interpreter = 'latex';
plot3(0, 0, 0, '.k', 'MarkerSize', 20)
plot3([0 XY1(i,1)], [0 XY1(i,2)], [0, XY1(i,3)], '-r', 'Linewidth', 4)
plot3(XY1(i,1), XY1(i,2), XY1(i,3), '.k', 'MarkerSize', 20)
plot3([XY1(i,1) XY2(i,1)], [XY1(i,2) XY2(i,2)], [XY1(i,3) XY2(i,3)], '-b', 'Linewidth', 4)
plot3(XY2(i,1), XY2(i,2), XY2(i,3), '.k', 'MarkerSize', 20)
plot3([XY2(i,1) XY3(i,1)], [XY2(i,2) XY3(i,2)+0.095], [XY2(i,3) XY3(i,3)],'-g', 'Linewidth', 4)
plot3(XY3(i,1), XY3(i,2)+0.095, XY3(i,3), '.k', 'MarkerSize', 20)
plot3([XY3(i,1) XYe(i,1)], [XY3(i,2)+0.095 XYe(i,2)], [XY3(i,3) XYe(i,3)], '-m', 'Linewidth', 4)
plot3(XYd(1:i, 1), XYd(1:i, 2), XYd(1:i, 3), '--r', 'LineWidth', 3)
plot3(XYe(1:i, 1), XYe(1:i, 2), XYe(1:i, 3), '-b', 'LineWidth', 1.5)
plot3(p_i(1), p_i(2), p_i(3), 'k*', 'MarkerSize', 16)
text(p_i(1) + 0.05, p_i(2), p_i(3)+0.05, '$p_i$', 'Interpreter', 'latex', 'FontSize', 16)
plot3(p_int(1), p_int(2), p_int(3),'k*', 'MarkerSize', 16)
text(p_int(1), p_int(2), p_int(3)+0.05, '$p_{int}$', 'Interpreter', 'latex', 'FontSize', 16)
plot3(p_f(1), p_f(2), p_f(3),'k*', 'MarkerSize', 16)
text(p_f(1) + 0.05, p_f(2), p_f(3)+0.05, '$p_f$', 'Interpreter', 'latex', 'FontSize', 16)
xlabel('x [m]','Interpreter','latex')
ylabel('y [m]','Interpreter','latex')
zlabel('z [m]','Interpreter','latex')
title("SENZA FUNZIONALE",'Interpreter','latex')
plot3(0, 0, 0,'.k','MarkerSize',20)
plot3([0 XY1_man(i,1)], [0 XY1_man(i,2)], [0 XY1_man(i,3)],'-r','Linewidth',4)
plot3(XY1_man(i,1), XY1_man(i,2), XY1_man(i,3),'.k','MarkerSize',20)
plot3([XY1_man(i,1) XY2_man(i,1)], [XY1_man(i,2) XY2_man(i,2)], [XY1_man(i,3) XY2_man(i,3)],'-b','Linewidth',4)
plot3(XY2_man(i,1), XY2_man(i,2), XY2_man(i,3),'.k','MarkerSize',20)
plot3([XY2_man(i,1) XY3_man(i,1)], [XY2_man(i,2) XY3_man(i,2)+0.095], [XY2_man(i,3) XY3_man(i,3)],'-g','Linewidth',4)
plot3(XY3(i,1), XY3(i,2)+0.095, XY3(i,3),'.k', 'MarkerSize', 20)
plot3([XY3(i,1) XYe(i,1)], [XY3(i,2)+0.095 XYe(i,2)], [XY3(i,3) XYe(i,3)],'-m', 'Linewidth', 4)
plot3(XYd(1:i,1), XYd(1:i,2), XYd(1:i,3),'--r','LineWidth',3)
plot3(XYe_man(1:i,1), XYe_man(1:i,2), XYe_man(1:i,3),'-b','LineWidth',1.5)
plot3(p_i(1), p_i(2), p_i(3), 'k*', 'MarkerSize', 16)
text(p_i(1) + 0.05, p_i(2), p_i(3)+0.05, '$p_i$', 'Interpreter', 'latex', 'FontSize', 16)
plot3(p_int(1), p_int(2), p_int(3),'k*', 'MarkerSize', 16)
text(p_int(1), p_int(2), p_int(3)+0.05, '$p_{int}$', 'Interpreter', 'latex', 'FontSize', 16)
plot3(p_f(1), p_f(2), p_f(3),'k*', 'MarkerSize', 16)
text(p_f(1) + 0.05, p_f(2), p_f(3)+0.05, '$p_f$', 'Interpreter', 'latex', 'FontSize', 16)
xlabel('x [m]','Interpreter','latex')
ylabel('y [m]','Interpreter','latex')
ylabel('z [m]','Interpreter','latex')
title("CON FUNZIONALE",'Interpreter','latex')