% Function: main_integrator_mpc
% Simulating integrator+delay process with PI-controller, and MPC
% controller
% and the Skogestad settings

clear all

% Disturbance and reference, at time t=0.
v=0; r=1;
tau=1; 
kg=1;

% Simulation time horizon
dt=0.1; t=0:dt:80; t=t'; N=length(t);

% Number of delay-states for implementing delay
nt=tau/dt;

% PI controller settings
itun=5;
if itun==1
  Kp=1/(2*kg*tau); Ti=8*tau;      % Skogestad setting
elseif itun == 2
  Kp=1/(2*kg*tau); Ti=5.83*tau;
elseif itun ==5     % exp(-tau*s)=1-tau*s
    c=2.75; % Gm=3.15
    c=2.6;
    tau0=c*tau; 
    Ti=2*tau0+tau;
    Kp=Ti/(kg*(tau0^2+Ti*tau));  
end

% State space model, for delay + dynamic model
[Atau,Btau,Dtau] = ss4delay(tau,dt);  % ss model for delay
A=1; B=dt*kg; D=1;
[At,Bt,Dt]=ss2aug4delay(A,B,D,Atau,Btau,Dtau); Ct=[dt;zeros(nt,1)];
q=0.00005; Rdu=1;
[G1,G2,At2,Bt2,Dt2]=dlqdu_pi(At,Bt,Dt,q,Rdu);  % MPC with infinite horizon

% MPC matrices
L=50; % Prediction horizon
[HdL,OL,OLB]=ss2h(At2,Bt2,Dt2,0,L,0);
FdL=[OLB HdL];
q2=0.001;
Qt=q2*eye(L); P=Rdu*eye(L);
H=FdL'*Qt*FdL+P;

% Type of controller: ireg=1 (PI), ireg=2 (MPC)
ireg=3;

% Initial values
x=0; z=-v; u_old=Kp; Iae=0; 
xt=zeros(1+nt,1);

xt_old=xt; y_old=0;
for k=1:N       
       
    y=Dt*xt;
    
    if k>floor(40/dt); v=0; r=0.5; end
    
    if ireg == 1     % PI controller
        e=r-y; 
        u=Kp*e+z;
        z=z+dt*Kp*e/Ti;
    
    elseif ireg == 2 % MPC/optimal controller, infinite pred. horizon
        dx=xt-xt_old;
        u=u_old+G1*dx+G2*(y_old-r);
        xt_old=xt; u_old=u; y_old=y;
    elseif ireg == 3 % MPC controller, unconstrained
        rf=ones(L,1)*r;
        xt2=[xt-xt_old;y_old];     
        y_old=y; 
        xt_old=xt;
 
        pl=OL*At2*xt2;
        f=FdL'*Qt*(pl-rf);

        duf=-inv(H)*f;
        u=u_old+duf(1);
        u_old=u;
    end
    
       
    % Save for plotting
    Y(k,1)=y;
    U(k,1)=u;
    R(k,1)=r;
                
    xt=At*xt+Bt*u+Ct*v;
    
    % Process
    %x=x+dt*kg*(u+v);
end


figure(1)
plot(t,Y,'-r',t,R,'-.')
legend('Output, y','Reference, r')

figure(2)
subplot(211), plot(U)
title('Control, u');
subplot(212), plot(diff(U))
title('Control, du');
