global sol
run ../../parametry.m

dt=0.002;
%% vizualizace
p=sol.parameters;
tf=sol.x(end);
dt_su=0.001;
t=[0:dt_su:tf];
% a=-p(1)-p(3)+(-p(2)-p(4))*cos(pi*t/tf);
% for i=2:5
%     a=a+p(i-1)*cos(i*pi*t/tf);
% end
% aaa=[t(1:end); a]';
% a2=[0 diff(a)/dt_su];
% a3=[0 diff(a2)/dt_su];
% acc=[t(1:end) ; a3]';

u=pi^2/tf^2*(p(2)+p(4))*cos(pi*t/tf);
for i = 2:5
    u=u-(i*pi/tf)^2*p(i-1)*cos(i*pi*t/tf);
end
acc=[t;u]';


th0i=0;
th1i=-pi;
th2i=-pi+0.00001;

open('doublePendulumSim_model_vizualizace.slx');
sim('doublePendulumSim_model_vizualizace.slx');
save('sol','sol')