clear all;
close all;

pinit;

Ts=0.1;
% zadany system
num=poly([-5 64 -complex(28,35) -complex(28,-35)]);
denum=poly([-complex(1,10) -complex(1,-10) -complex(-1,25) -complex(-1,-25) 3]);
sys=tf(num,denum);

[M,N,C,D]=ssdata(c2d(sys,Ts));

Ns = size(M, 1);
Nu = size(N, 2);
I=eye(Ns);

% vahove matice
Q = diag([1, 1, 1, 1, 1])
R=1e+5

P = Q;

x = 70;
Kh = zeros(Ns,x);
for t = x : -1 : 1
   K=inv(R+N'*P*N)*N'*P*M;
   P=M'*P*M+Q-M'*P*N*K;
   Kh(:,t) = K';
end

figure(1);
plot(Kh');
hold on;

[P,L,Kdare] = dare(M,N,Q,R);
Kdare

gama=1/sqrt(1+N'*P*N);
1/(1+gama)
1/(1-gama)
asin(gama)


figure(1);
plot(0,Kdare,'o');

%K_x = Kh(:,1)'
K_x = Kdare;
VLCisla_MNK = eig(M-N*K_x)
sim('lq1'); 

figure(10); 

%otevrena smycka
%OL = ss(M,N,K_x,0);
OL=K_x*inv(z*I-M)*N;

figure(2); grid; bode(OL,Ts,'b'); hold on;
figure(3); grid; nakresli(AkcniZasah,'Akcni zasah','b'); hold;
figure(4); grid; nakresli(Vstup, 'Vstup do soustavy','b'); hold;
figure(5); grid; nakresli(Vystup, 'Vystup','b');hold;

figure(6); grid; subplot(2,3,1);grid;  nakresli_stav(Stavy,1,'x1','b'); hold;
figure(6); grid; subplot(2,3,2);grid;  nakresli_stav(Stavy,2,'x2','b'); hold;
figure(6); grid; subplot(2,3,3);grid;  nakresli_stav(Stavy,3,'x3','b'); hold;
figure(6); grid; subplot(2,3,4);grid;  nakresli_stav(Stavy,4,'x4','b'); hold;
figure(6); grid; subplot(2,3,5);grid;  nakresli_stav(Stavy,5,'x5','b'); hold;


%------------------
Q = diag([1000, 1, 1, 1, 1])
R=1e+3

P = Q;

x = 70;
Kh = zeros(Ns,x);
for t = x : -1 : 1
   K=inv(R+N'*P*N)*N'*P*M;
   P=M'*P*M+Q-M'*P*N*K;
   Kh(:,t) = K';
end

[P,L,Kdare] = dare(M,N,Q,R);
Kdare

gama=1/sqrt(1+N'*P*N);
1/(1+gama)
1/(1-gama)
asin(gama)

figure(1);
plot(0,Kdare);

%K_x = Kh(:,1)'
K_x = Kdare;
VLCisla_MNK = eig(M-N*K_x)
sim('lq1'); 

%otevrena smycka
%OL = ss(M,N,K_x,0);
OL=K_x*inv(z*I-M)*N;

figure(2); grid; bode(OL,Ts,'r'); 
figure(3); grid; nakresli(AkcniZasah,'Akcni zasah','r'); 
figure(4); grid; nakresli(Vstup, 'Vstup do soustavy','r'); 
figure(5); grid; nakresli(Vystup, 'Vystup','r');

figure(7); grid; subplot(2,3,1);grid;  nakresli_stav(Stavy,1,'x1','r');
figure(7); grid; subplot(2,3,2);grid; nakresli_stav(Stavy,2,'x2','r');
figure(7); grid; subplot(2,3,3);grid; nakresli_stav(Stavy,3,'x3','r');
figure(7); grid; subplot(2,3,4);grid; nakresli_stav(Stavy,4,'x4','r');
figure(7); grid; subplot(2,3,5);grid; nakresli_stav(Stavy,5,'x5','r');


%------------------
Q = diag([100, 100, 100, 100, 100])
R=1e+4

P = Q;

x = 70;
Kh = zeros(Ns,x);
for t = x : -1 : 1
   K=inv(R+N'*P*N)*N'*P*M;
   P=M'*P*M+Q-M'*P*N*K;
   Kh(:,t) = K';
end

[P,L,Kdare] = dare(M,N,Q,R);
Kdare

gama=1/sqrt(1+N'*P*N);
1/(1+gama)
1/(1-gama)
asin(gama)


figure(1);
plot(0,Kdare);

%K_x = Kh(:,1)'
K_x = Kdare;
VLCisla_MNK = eig(M-N*K_x)
sim('lq1'); 

%otevrena smycka
%OL = ss(M,N,K_x,0);
OL=K_x*inv(z*I-M)*N;

figure(2); grid on; bode(OL,Ts,'k'); 
figure(3); grid; nakresli(AkcniZasah,'Akcni zasah','k'); 
figure(4); grid; nakresli(Vstup, 'Vstup do soustavy','k'); 
figure(5); grid; nakresli(Vystup, 'Vystup','k');

figure(8); subplot(2,3,1);grid; nakresli_stav(Stavy,1,'x1','k');
figure(8); grid; subplot(2,3,2);grid ;nakresli_stav(Stavy,2,'x2','k');
figure(8); grid; subplot(2,3,3);grid; nakresli_stav(Stavy,3,'x3','k');
figure(8); grid; subplot(2,3,4);grid; nakresli_stav(Stavy,4,'x4','k');
figure(8); grid; subplot(2,3,5);grid; nakresli_stav(Stavy,5,'x5','k');