% Letacek, Tomasek
% prediktivni rizeni

close all;
Ts = 0.01;
delka = 5;

A = [0 1 0 0; 0 0 1 0;0 0 0 1; 10^-11 -24010 -2539 -54];
B = [ 0;0;0;31940];
C = [1 0 0 0];
D = [0];

sys=ss(A,B,C,D);
dsys=c2d(sys,Ts,'zoh');
[M,N,C,D]=ssdata(dsys);

Tp = 40;

V = matV(Tp,M,C);
S = matS(Tp,M,N,C,D);
% zrychlit cyklus
Spom=zeros(Tp,delka-1);
for i=1:delka
   for j=1:5
      Spom((i-1)*5+j,i)=1;
   end   
end
for i=5*delka:Tp
   Spom(i,delka)=1;
end

Spom
S = S * Spom;

R = 1*eye(delka);						% volime
Q = 100*eye(Tp);				% volime

x=zeros(size(M,1),1);  	% volime
ref = 1;
ys = V*x;

W = zeros(delka,delka);
z = zeros(delka,1);

H = S'*Q*S + R;
H = (H+H')/2;

x_hist=zeros(size(M,1),Tp);
y_hist=zeros(Tp,1);
u_hist=zeros(Tp,1);

pom_vek = ones(1,Tp)';

for i=1:100
   J = ((V*x-ref)'*Q*S);
   us = qp(H, J', W, z, -5*(ones(1,delka)'),5*ones(1,delka)');
   u = us(1);
   
   y = C*x + D*u;
	x = M*x + N*u;
     
   figure(1);
   subplot(2, 2, 1);
   hold on;
   plot(i,ref,'.');
   title('ref');
   subplot(2, 2, 2);
   hold on;
   plot(i,u,'.');
   title('u');
   subplot(2, 2, 3);
   hold on;
   plot(i,x,'.');
   title('x');
   subplot(2, 2, 4);
   hold on;
   plot(i,y,'.');   
   title('y');
   drawnow;
end