clear all;
%close all;

%------------- System --------------
P = tf([10],[1 -1 -1 1]);

Ts = 0.1;
Pd = c2d(P,Ts);
[M,N,C,D]=ssdata(Pd);

%---------- Inicializace ---------------
%Doba predikce
Tp = 50;

for i=0:1:Tp-1
   V(i+1,:) = C*M^i;
end

S = zeros(Tp);

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

hnus(:,1)=[ones(3,1);zeros(3,1);zeros(3,1);zeros(41,1)];
hnus(:,2)=[zeros(3,1);ones(3,1);zeros(3,1);zeros(41,1)];
hnus(:,3)=[zeros(3,1);zeros(3,1);ones(3,1);zeros(41,1)];
hnus(:,4)=[zeros(3,1);zeros(3,1);zeros(3,1);ones(41,1)];


S1 = S*hnus;


Q = eye(Tp)*1000;

R = eye(Tp);
R1 = eye(4);

%---------------- Simulace ------------------
n = 300;
x = [0 0 0]';
%ref = [5*ones(1,n/2),22*ones(1,n/2)];
ref=[zeros(1,n/3),ones(1,2*n/3)];

A = -inv(S'*Q*S+R)*(S'*Q)*V;
B = inv(S'*Q*S+R)*(S'*Q);
Kx = A(1,:); Kr = sum(B(1,:));
for k=1:n
   u = Kx*x+Kr*ref(k);
   y = C*x + D*u;
   x = M*x + N*u;
   Vu(:,k)=u;
   Vy(k)=y;
   Vx(:,k)=x;
end
%------------- Matice uzavrene smycky -----------
M_MPC = M-N*Kx;
N_MPC = Kr*N;
C_MPC = C;
D_MPC = Kr*D;
sys = ss(M_MPC,N_MPC,C_MPC,D_MPC,Ts);

clear A,B,Kx,Kr;
%--------------- Simulace pro redukci vstupu ----------
n = 300;
x = [0 0 0]';
%ref = [5*ones(1,n/2),22*ones(1,n/2)];
ref=[zeros(1,n/3),ones(1,2*n/3)];

A = -inv(S1'*Q*S1+R1)*(S1'*Q)*V;
B = inv(S1'*Q*S1+R1)*(S1'*Q);
Kx = A(1,:); Kr = sum(B(1,:));
for k=1:n
   u = Kx*x+Kr*ref(k);
   y = C*x + D*u;
   x = M*x + N*u;
   Vu1(:,k)=u;
   Vy1(k)=y;
   Vx1(:,k)=x;
end
%------------- Matice uzavrene smycky -----------
M_MPC = M-N*Kx;
N_MPC = Kr*N;
C_MPC = C;
D_MPC = Kr*D;
sys1 = ss(M_MPC,N_MPC,C_MPC,D_MPC,Ts);

%-------------------- Vysledky -----------------
figure(5);
subplot(2,1,1); plot(1:n,Vu,'b',1:n,Vu1,'g'); title('Ridici velicina'); xlabel('k'); ylabel('u');
legend('u(k)','u(k) pro redukci stupnu volnosti');
subplot(2,1,2); plot(1:n,Vy,'b',1:n,Vy1,'g',1:n,ref,'r'); title('Regulovana velicina'); xlabel('k'); ylabel('y,ref');
legend('y(k)','y(k) pro redukci stupnu volnosti','Reference',4);

figure(6);
bode(sys,'b',sys1,'g');