% process's dynamics
na=7;
nb=9;

A=[1,-0.7836,-0.3322,-0.007717,0.04919,0.02815,0.0309,0.03564];
B=[0.003047,0.004778,0.008546,0.005764,0.004639,0.002812,-0.000382,0.001464,-0.003613,-0.0006973];

% initialization
E(1)=1;

for i=1:na
    F(1,i)=-A(i+1)
end

G(1)=E(1)*B(1,1);

for i=1:nb
    H(1,i)=B(i+1)
end

% prediction and control step
P=100;
M=1;


%  E,F,G and H of step 2 to P
for j=2:P
    
    E(j)=F(j-1,1);
        
    for i=1:na-1
        F(j,i)=F(j-1,i+1)-E(j)*A(i+1);
    end
    F(j,na)=-E(j)*A(na+1);
    
    G(j)=E(j)*B(1)+H(j-1,1);
    
    for i=1:nb-1
        H(j,i)=H(j-1,i+1)+E(j)*B(i+1);
    end
    H(j,nb)=E(j)*B(nb+1);
end

% deltaF for estimating feedback and predictive control 
for j=1:P
    for i=1:na
        if j==1
            deltaF(j,i)=F(j,i);
        else
            deltaF(j,i)=F(j,i)-F(j-1,i);
        end
    end
end

% G0 for estimating feedback when U=[u(k+M-1),...,u(k+1)]
for j=1:P
    for i=1:P
        if i<=j
            G0(j,i) = G(i);
        else 
            G0(j,i) = 0;
        end
    end
end

% G1 for gloable predictive control when U=[u(k+M-1),...,u(k+1)]
for j=1:P
    for i=1:P
        if i<=j
            G1(j,P+1-i) = G(j+1-i);
        else
            G1(j,P+1-i) = 0;
        end
    end
end 

% deltaG1 for gloable predictive control when U=[u(k+M-1),...,u(k+1)]
for j=1:P
    for i=1:P
        if j==1
            deltaG1(j,i) = G1(j,i);
        else
            deltaG1(j,i) = G1(j,i)-G1(j-1,i);
        end
    end
end

% G2 for last output
%G2=[1;0;0;0;0;0;0;0;0;0];


% deltaH for past control signals
for j=1:P
    for i=1:nb
        if j==1
            deltaH(j,i)=H(j,i);
        else
            deltaH(j,i)=H(j,i)-H(j-1,i);
        end
    end
end

% LAMBDA is a set of lambdas for U=[u(k+M-1),...,u(k+1)]
for j=1:M
    for i=1:M
        if j==i
            LAMBDA(j,i)=0.5;
        else
            LAMBDA(j,i)=0;
        end
    end
end

% deltaKP = kp * (kp*deltaG1^T*deltaG1 + ki*G1^T*G1 + LAMBDA)^-1 * deltaG1^T 
% deltaKI = ki * (kp*deltaG1^T*deltaG1 + ki*G1^T*G1 + LAMBDA)^-1 * G1^T
kp = 0;
ki = 1;

%deltaKP = kp*inv(kp*deltaG1'*deltaG1 + ki*G1'*G1 + LAMBDA)*deltaG1';
%deltaKI = ki*inv(kp*deltaG1'*deltaG1 + ki*G1'*G1 + LAMBDA)*G1';

%fof P=12,M=1,lambda=0.5
deltaKI = ki*inv(ki*G1'*G1 + LAMBDA)*G1';