% main
% parametry systemu
L = 0.02; % L [m]
parametry{1} = 0.1; % m [kg]
parametry{2} = 0.3; % M [kg]
parametry{3} = L; % L [m]
parametry{4} = 9.81; % g [m/s^2]
parametry{5} = 3; % b [N.s/m]
parametry{6} = 1e-3; % Is [kg.m^2]

% pocatecni poloha
X0=[0;0;pi/8;0]; % [x,dx,phi,dphi]
% simulace
[t,x]=ode45(@(t,x) DynSystem(t,x,0,parametry),0:1e-5:10,X0);

% zobrazeni stavu
figure;
subplot(2,2,1);plot(t,x(:,1));xlabel('t [s]');ylabel('x [m]');title('poloha vozicku');grid on
subplot(2,2,2);plot(t,x(:,3)*180/pi);xlabel('t [s]');ylabel('\phi [°]');title('natoceni kyvadla');grid on
subplot(2,2,3);plot(t,x(:,2));xlabel('t [s]');ylabel('dx [m/s]');title('rychlost vozicku');grid on
subplot(2,2,4);plot(t,x(:,4));xlabel('t [s]');ylabel('d\phi [rad/s]');title('uhlova rychlost kyvadla');grid on

% animace
figure;
% pocatecni poloha
plot(X0(1),0,'sk');hold on % umoznuje prikreslit dalsi krivky
plot([X0(1) X0(1)+L*sin(X0(3))],[0 L*cos(X0(3))],'k');
% pro animaci
a1 = plot(0,0,'sg');
a2 = plot(0,0,':g');
a3 = plot(0,0,'b');
axis equal
% animace
for kk=1:100:length(t)
    set(a1,'Xdata',x(kk,1),'Ydata',0);
    set(a2,'Xdata',[X0(1) x(kk,1)],'Ydata',[0 0]);
    set(a3,'Xdata',[x(kk,1) x(kk,1)+L*sin(x(kk,3))],'Ydata',[0 L*cos(x(kk,3))]);
    drawnow; % vykresli graf
end

[A,B,C,D,R,O,lambda] = LinSystem(parametry);

if rank(R)==size(A,2)
    disp('system je riditelny');
else
    disp('system je neriditelny');
end
if rank(O)==size(A,2)
    disp('system je pozorovatelny');
else
    disp('system je nepozorovatelny');
end
if sum(real(lambda)>0)>0
    disp('system je nestabilni');
elseif sum(real(lambda)==0)>0
    disp('system je na hranici stability');
else
    disp('system je stabilni');    
end

sim('MainSimulink',0:1e-5:1);

figure;
plot(simout(:,1),simout(:,2));
figure;
plot(simout(:,1),simout(:,3));