% MPC course
% Solution proposal for Exercise 1 step b
%
a=0.7; b=0.8; d=1; x0=0;

q=3; p=1;
% Exercise 2a
% Solution proposal for simple scalar system controlled by a MPC controller
% with varying horizon, L=1, L04

N=50;
r=ones(N+1,1); r(26:50)=ones(25,1)*0.5;

Y=zeros(N,1); U=zeros(N,1);

hl=4;

if hl==1
    g=-q*b/(p+q*b^2);
elseif hl==4
    Q=diag([q q q q]); P=diag([p p p p]);
    O4=[d;d*a;d*a^2;d*a^3];
    H4=[0 0 0;d*b,0,0;d*a*b,d*b,0;d*a^2*b,d*a*b,d*b];
    F4=[O4*b H4]
    G=-inv(F4'*Q*F4+P)*F4'*Q;
    O4A=O4*a;
end

x=x0;
for k=1:N-3
    y=x;                    % Process measurements
    
    if hl==1
        u=g*(a*y-r(k+1))        % Computing control input
    elseif hl==4
        u=G(1,:)*(O4A*x-r(k+1:k+4));
    end
    
%---------------------------  Storing variables
    U(k)=u;
    Y(k)=y; 
%---------------------------     
    x=a*x+b*u;              % Update the state equation
end
%Y(N)=y;
    
subplot(211), plot(U), grid
title('Simulation of Exercise 1')
ylabel('u_k^\ast')
subplot(212), plot([r(1:N,1) Y]), grid
ylabel('r_k and y_k')
xlabel('Discrete time   0 \leq k \leq N')