% soustava
close all
clear all

cit = [10];
jme = [1    -1    -1     1];
%cit=[1];
%jme=poly([1 1 1]);

[A,B,C,D] = tf2ss(cit,jme);

% diskretizace
Ts = 0.1;
[Ad, Bd, Cd, Dd] = ssdata(c2d(ss(A,B,C,D),Ts));

figure(1);
hold on;
grid on;
figure(3);
hold on;
grid on;


Tp = 50;
col='r';
   
Q = 10*eye(Tp);
R = 1*eye(Tp);
  N = 300;
   
	ref = zeros(1,N+Tp);
	for i=N/3:N
  		ref(i)=1;
	end

	[uh1, xh1, yh1, Kx1, Kr1] = PR_bezomezeni(Tp, R, Q, ref, N, Ad, Bd, Cd, Dd);
      
	A_MPC = Ad-Bd*Kx1;
	B_MPC = Kr1*Bd;
	C_MPC = Cd;
	D_MPC = Kr1*Dd;  % a D = 0
   O=ss(A_MPC,B_MPC,C_MPC,D_MPC,Ts);
   figure(3); bode(O,col);   
   
   %%%%%%%%%%%%%%%%%%%%%%%%%%
col='g';   

Q = 100*eye(Tp);
	R = 1*eye(Tp);
   N = 300;
   
	ref = zeros(1,N+Tp);
	for i=N/3:N
  		ref(i)=1;
	end

	[uh2, xh2, yh2, Kx2, Kr2] = PR_bezomezeni(Tp, R, Q, ref, N, Ad, Bd, Cd, Dd);
      
	A_MPC = Ad-Bd*Kx2;
	B_MPC = Kr2*Bd;
	C_MPC = Cd;
	D_MPC = Kr2*Dd;  % a D = 0
   O=ss(A_MPC,B_MPC,C_MPC,D_MPC,Ts);
   figure(3); bode(O,col);   
%%%%%%%%%%%%%%%%%%%%%%%%%%%5   

col='b';
	Q = 1000*eye(Tp);
	R = 1*eye(Tp);
   N = 300;
   
	ref = zeros(1,N+Tp);
	for i=N/3:2*N/3
  		ref(i)=1;
	end

	[uh3, xh3, yh3, Kx3, Kr3] = PR_bezomezeni(Tp, R, Q, ref, N, Ad, Bd, Cd, Dd);
      
	A_MPC = Ad-Bd*Kx3;
	B_MPC = Kr3*Bd;
	C_MPC = Cd;
	D_MPC = Kr3*Dd;  % a D = 0
   O=ss(A_MPC,B_MPC,C_MPC,D_MPC,Ts);
   figure(3); bode(O,col);      


figure(1); subplot(2,1,1),plot(1:N,yh1,'r',1:N,yh2,'g',1:N,yh3,'b',1:N,ref(1:N),'y'); title('Regulovana velicina'); legend('Tp=50, Q=10','Q=100','Q=1000');
  figure(1); subplot(2,1,2),plot(1:N,uh1,'r',1:N,uh2,'g',1:N,uh3,'b'), hold on; title('Ridici velicina');