%GPC
close all;
clear all;
%u-T1
num1=-1.4261;
den1=conv([10 1],[30 1]);
sys1=tf(num1,den1);
%T1-T2
num2=0.3181;
den2=conv([30 1],[90 1]);
sys2=tf(num2,den2);
%T-T2
num3=0.8022;
den3=conv([90 1],[90 1]);
sys3=tf(num3,den3);
%T-u
sys=sys1*sys2*sys3;
%sample time
Ts=40;
sysd=c2d(sys,Ts,'zoh');
set(sysd,'variable','z^-1');
A=sysd.den{1};   
B=sysd.num{1};
n_a=length(A)-1;
n_b=length(B)-1;
%predictive step
N_p=15;
N_u=1;
G_1=cell(N_p,1);
F=cell(N_p,1);
G=zeros(N_p,N_u);
for i=1:N_p
    [E,F{i},G_1{i}]=DiophEquSlvrV2(A,B,i);
    G(i,1:min(1,N_u))=G_1{i}(i:-1:max(1,i-N_u+1));
end
lambba=1;
GG=inv(G'*G+lambba*eye(N_u))*G';
K=GG(1,:);
%simulation step 
S_Time=300;
y_actual=0;
u=0;
deltau=0;
for k=1:S_Time
    time(k)=k*Ts;
%     if k<50
        r(k)=1;
        c(k)=0.1;
%     elseif k<100
%         r(k)=0;
%         c(k)=0.1;
%     elseif k<150
%         r(k)=1;
%         c(k)=0.5;
%     elseif k<200
%         r(k)=0;
%         c(k)=0.5;
%     elseif k<250
%         r(k)=1;
%         c(k)=0.9;
%     else
%         r(k)=0;
%         c(k)=0.9;
%     end 
    y_actual(k)=B*u(max(1,[k-1:-1:k-1-n_b]))'-A(2:end)*y_actual(max(1,[k-1:-1:k-n_a]))';
    %reference trajectory calculation
    w(1)=y_actual(k);
    for j=2:N_p
        w(j)=(c(k)^(j-1))*w(1)+(1-(c(k)^(j-1)))*r(k)
    end  
    for j=1:N_p
        f(j)=F{j}*[y_actual(max(1,[k:-1:k-n_a]))]'+ G_1{j}(j+1:end) * [deltau(max(1,[k-1:-1:k-n_b]))]';;
    end 
    FF=f';
    W=w';
    deltau(k)=K*(W-FF);
    u(k)=u(max(1,k-1))+deltau(k);
end
figure;
subplot(211);
plot(time,r,'r--',time,y_actual,'b');
legend('Y-set','Y-actual');
title('System Y-set and Y-actual');
grid on;
subplot(212);
plot(time,u,'r');
title('Control action');
xlabel('Time (sec)');
grid on;