% prediktivni rizeni

Ts = 0.1;
sys=tf([1],[1 -3 3 1]);
dsys=c2d(sys,Ts,'zoh');
[M,N,C,D]=ssdata(dsys);

Tp = 40;

%V=zeros(Tp,size(C,2));
%S=zeros(Tp);

%for i=1:Tp
%   V(i,:) = C*M^(i-1);
%   S(i,i)=D;
%   for j=(i+1):Tp
%      S(j,i) = C*M^(j-2)*N;
%   end
%end

%V
%S

V = matV(Tp,M,C);
S = matS(Tp,M,N,C,D);

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

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

AA=inv(S'*Q*S+R)*(S'*Q*V);  %x === x(k)
BB=inv(S'*Q*S+R)*(S'*Q);

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

for i=1:Tp
   u = -AA(1,:)*x+sum(BB(1,:))*ref;
   y = C*x + D*u;
   x_hist(:,i)= x;
   y_hist(i,1) = y;
   u_hist(i,1) = u;
   x = M*x + N*u;
end

figure(1);
plot(u_hist);
figure(2);
plot(y_hist);