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
Tp1 = 10;
Tp2 = 50;
Tp3 = 100;
Tp = 50;

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

S1 = zeros(Tp1);

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

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

S2 = zeros(Tp2);

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

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

S3 = zeros(Tp3);

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

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

Qt1 = eye(Tp)*10;
Qt2 = eye(Tp)*100;
Qt3 = eye(Tp)*1000;
Q1 = eye(Tp1)*100;
Q2 = eye(Tp2)*100;
Q3 = eye(Tp3)*100;

R1 = eye(Tp1);
R2 = eye(Tp2);
R3 = eye(Tp3);
R = eye(Tp);

%---------------- 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(S1'*Q1*S1+R1)*(S1'*Q1)*V1;
B = inv(S1'*Q1*S1+R1)*(S1'*Q1);
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);
clear A,B,Kx,Kr;



A = -inv(S2'*Q2*S2+R2)*(S2'*Q2)*V2;
B = inv(S2'*Q2*S2+R2)*(S2'*Q2);
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;
   Vu2(:,k)=u;
   Vy2(k)=y;
   Vx2(:,k)=x;
end
%------------- Matice uzavrene smycky -----------
M_MPC = M-N*Kx;
N_MPC = Kr*N;
C_MPC = C;
D_MPC = Kr*D;
sys2 = ss(M_MPC,N_MPC,C_MPC,D_MPC,Ts);
clear A,B,Kx,Kr;



A = -inv(S3'*Q3*S3+R3)*(S3'*Q3)*V3;
B = inv(S3'*Q3*S3+R3)*(S3'*Q3);
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;
   Vu3(:,k)=u;
   Vy3(k)=y;
   Vx3(:,k)=x;
end
%------------- Matice uzavrene smycky -----------
M_MPC = M-N*Kx;
N_MPC = Kr*N;
C_MPC = C;
D_MPC = Kr*D;
sys3 = ss(M_MPC,N_MPC,C_MPC,D_MPC,Ts);
clear A,B,Kx,Kr;



A = -inv(S'*Qt1*S+R)*(S'*Qt1)*V;
B = inv(S'*Qt1*S+R)*(S'*Qt1);
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;
   Vu4(:,k)=u;
   Vy4(k)=y;
   Vx4(:,k)=x;
end
%------------- Matice uzavrene smycky -----------
M_MPC = M-N*Kx;
N_MPC = Kr*N;
C_MPC = C;
D_MPC = Kr*D;
sys4 = ss(M_MPC,N_MPC,C_MPC,D_MPC,Ts);
clear A,B,Kx,Kr;



A = -inv(S'*Qt2*S+R)*(S'*Qt2)*V;
B = inv(S'*Qt2*S+R)*(S'*Qt2);
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;
   Vu5(:,k)=u;
   Vy5(k)=y;
   Vx5(:,k)=x;
end
%------------- Matice uzavrene smycky -----------
M_MPC = M-N*Kx;
N_MPC = Kr*N;
C_MPC = C;
D_MPC = Kr*D;
sys5 = ss(M_MPC,N_MPC,C_MPC,D_MPC,Ts);
clear A,B,Kx,Kr;



A = -inv(S'*Qt3*S+R)*(S'*Qt3)*V;
B = inv(S'*Qt3*S+R)*(S'*Qt3);
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;
   Vu6(:,k)=u;
   Vy6(k)=y;
   Vx6(:,k)=x;
end
%------------- Matice uzavrene smycky -----------
M_MPC = M-N*Kx;
N_MPC = Kr*N;
C_MPC = C;
D_MPC = Kr*D;
sys6 = ss(M_MPC,N_MPC,C_MPC,D_MPC,Ts);
clear A,B,Kx,Kr;


%-------------------- Vysledky -----------------
figure(1);
subplot(2,1,1); plot(1:n,Vu1,'b',1:n,Vu2,'g',1:n,Vu3,'m'); title('Ridici velicina'); xlabel('k'); ylabel('u'); legend('Q=100;Tp=10','Tp=50','Tp=100');
subplot(2,1,2); plot(1:n,Vy1,'b',1:n,Vy2,'g',1:n,Vy3,'m',1:n,ref,'r'); title('Regulovana velicina'); xlabel('k'); ylabel('u');
%subplot(2,2,1); plot(Vu2,'g');
%subplot(2,2,3); plot(1:n,Vy2,'g'); %legend('y','reference',4);
%subplot(2,2,1); plot(Vu3,'m'); hold off;
%subplot(2,2,3); plot(1:n,Vy3,'m'); hold off;
figure(2);

bode(sys1,'b',sys2,'g',sys3,'m'); legend('Q=100;Tp=10','Tp=50','Tp=100');

figure(3);

subplot(2,1,1); plot(1:n,Vu4,'b',1:n,Vu5,'g',1:n,Vu6,'m'); title('Ridici velicina'); xlabel('k'); ylabel('u'); legend('Q=10;Tp=50','Q=100','Q=1000');
subplot(2,1,2); plot(1:n,Vy4,'b',1:n,Vy5,'g',1:n,Vy6,'m',1:n,ref,'r'); title('Regulovana velicina'); xlabel('k'); ylabel('u');
%subplot(2,2,2); plot(Vu5,'g');
%subplot(2,2,4); plot(1:n,Vy5,'g'); %legend('y','reference',4);
%subplot(2,2,2); plot(Vu6,'m'); hold off;
%subplot(2,2,4); plot(1:n,Vy6,'m'); hold off;


%figure(2);
%subplot(1,2,1); bode(sys1,'b',sys2,'g',sys3,'m'); legend('Q=100;Tp=10','Tp=50','Tp=100');
%subplot(1,2,1); bode(sys2,'g');
%subplot(1,2,1); bode(sys3,'m'); hold off;

figure(4);

bode(sys4,'b',sys5,'g',sys6,'m'); legend('Q=10;Tp=50','Q=100','Q=1000');
%subplot(1,2,2); bode(sys5,'g');
%subplot(1,2,2); bode(sys6,'m'); hold off;

%figure(1);
%plot(Vy);

%figure(2);
%plot(Vx');

%figure(3);
%plot(Vu);