%==========================================================================
% SSY280 Model Predictive Control 2012
%
% Homework Assignment 2:
% MPC Control of a Linearized MIMO Well Stirred Chemical Reactor
% Revised 2013-02-10
%==========================================================================

%******************* Initialization block *********************************

clear;
clc
close all
tf=50;                  % number of simulation steps

%==========================================================================
% Process model
%==========================================================================

h = 1; % sampling time in minutes

A = [ 0.2681   -0.00338   -0.00728;
      9.7032    0.3279   -25.44;
         0         0       1   ];
B = [ -0.00537  0.1655;
       1.297   97.91 ;
       0       -6.637];
C = [ 1 0 0;
      0 1 0;
      0 0 1];
Bp = [-0.1175;
      69.74;
       6.637 ];

n = size(A,1); % n is the dimension of the state
m = size(B,2); % m is the dimension of the control signal
p = size(C,1); % p is the dimension of the measured output

d=0.01*[zeros(1*tf/5,1);ones(4*tf/5,1)]; %unmeasured disturbance trajectory

x0 = [0.01;1;0.1]; % initial condition of system's state

%==========================================================================
% Set up observer model
%==========================================================================

% Three cases to be investigated

example = 'c';
switch example
    case 'a'
        nd = 2;
        Bd = zeros(n,nd);
        Cd = [1 0;0 0; 0 1];
    case 'b'
        nd = 3;
        Bd = zeros(n,nd);
        Cd = [1 0 0;0 0 1;0 1 0];
    case 'c'
        nd=3;
        Bd = [zeros(3,2) Bp];
        Cd = [1 0 0;0 0 0;0 1 0];
end

% Verify detectability
detect=rank([eye(n)-A, -Bd;C,Cd]);
if detect==n+nd
    fprintf('System Detectable\n')
else
    fprintf('System Not Detectable\n')
end

% Augment the model with constant disturbances

Ae = [A,Bd;zeros(nd,n),eye(nd)];
Be = [B;zeros(nd,2)];
Ce = [C,Cd];

% Calculate observer gain
switch example
    case 'a'
        Le = place(Ae',Ce',[-0.01, -0.03, -0.02, -0.015, -0.025])';
    case 'b'
        Le=[-0.106146678301203 -0.00296211521208997 -0.0364757336637961;9.56640530077989 1.37652321422317 -25.5979722091198;-0.540036420958648 -0.0403551551790192 2.44451009284336;1.40982966694229 -0.000478200306218713 0.0297436133689198;0.534553573163769 0.0408494810943643 -0.418716295708000;-0.000839197290552221 7.57279394927729e-05 0.154576370720167];
    case 'c'
        Le = place(Ae',Ce',[-0.01, -0.03, -0.02, -0.015, -0.025,-0.01])';
end
%==========================================================================
% Prepare for computing steady state targets
%==========================================================================

% Select 1st and 3rd outputs as controlled outputs
H = [1 0 0;0 0 1];

% Matrices for steady state target calculation to be used later

As=[eye(n)-A, -B; H*C, zeros(2,2)];
Bs = @(dh) [Bd*dh;[0;0]-H*Cd*dh];

%==========================================================================
% Set up MPC controller
%==========================================================================

N=10;                   % prediction horizon
M=3;                    % control horizon

Q = diag([1 0.001 1]);  % state penalty
Pf = Q;                 % terminal state penalty
R = 0.01*eye(m);        % control penalty

    %=================================
    % Build Hessian Matrix
    %=================================

    Hess = zeros(n*N+M*m);
    Hess(1:n*N,1:n*N) = kron(eye(N),Q);
    Hess(n*N+1:n*N+M*m,n*N+1:n*N+M*m) = kron(eye(M),R);
    f = zeros(length(Hess),1);

    %==========================================
    % Equality Constraints
    %==========================================
    leftAeq=zeros(N);
    rightAeq=zeros(N,M);
    for i=2:N
        leftAeq(i,i-1)=-1;
    end
    for i = 1:N
        if i > M
            rightAeq(i,M)=1;
        else
            rightAeq(i,i)=1;
        end
    end
    leftAeq=kron(leftAeq,A)+eye(n*N);
    rightAeq=kron(rightAeq,-B);
    Aeq=[leftAeq,rightAeq];
    AA=[A;zeros(n*(N-1),n)];

    %==========================================
    % Inequality Constraints
    %==========================================

    Ain = [];
    Bin = [];

    %==============================================
    % Choose QP solver
    %==============================================

    solver = 'int';
    switch solver
        case 'int'
            options = optimset('Algorithm','interior-point-convex','Display','off');
        case 'set'
            options = optimset('Algorithm','active-set','Display','off');
    end

%******************* End of initialization ********************************

%==========================================================================
% Simulation
%==========================================================================

% Initialization

xk=x0;
xhat = zeros(n,1);
d0 = zeros(nd,1);
xi = [xhat; d0];
Output=[];
Input=[];
Disturbance=[];
time_vec=[];
uk=[0;0];

% Simulate closed-loop system

    for k = 1:tf

        %======================================
        % Update the observer state xhat(k|k-1)
        %======================================

        % YOUR CODE GOES HERE
        xi(:, k+1) = Ae * xi(:, k) + Be * uk + Le * (C* xk - [C, Cd] * xi(:, k));

        %==============================================
        % Update the process state x(k) and output y(k)
        %==============================================

        xk = A*xk + B*uk + Bp*d(k);
        yk = C*xk;

        %=========================================
        % Calculate steady state targets xs and us
        %=========================================

        dhat=xi(n+1:n+nd,k+1);
        %dhat=C*xk-C*xi(:,k+1);
        Bsk=Bs(dhat);
        xs_us=As\Bs(dhat);
        xs=xs_us(1:n);
        us=xs_us(n+1:n+m);

        %============================================
        % Solve the QP (for the deviation variables!)
        %============================================

        beq=AA*(xk-xs);
        xk-xs

        % NOTE THAT Hess IS USED FOR THE HESSIAN, NOT TO BE CONFUSED
        %   WITH H THAT IS USED FOR SELECTING CONTROLLED VARIABLES
        z = quadprog(Hess,f,Ain,Bin,Aeq,beq,[],[],[],options);


        % NOTE THAT YOU NEED TO GO FROM DEVIATION VARIABLES TO 'REAL' ONES!
        uk=z(N*n+1:N*n+m)+us;
        %===============================
        % Store current variables in log
        %===============================

        Input=[Input,uk];
        Output=[Output,yk];
        Disturbance=[Disturbance,dhat];
        time_vec=[time_vec,k];

   end % simulation loop

%==========================================================================
% Plot results
%==========================================================================


xHat = xi(1:3, 2:end);

figure(1)
outputBoundaries = [min([Output xHat], [], 2) max([Output xHat], [], 2)];
margin = abs(outputBoundaries(:, 1) - outputBoundaries(:, 2)) * 0.1;
ylims = [outputBoundaries(:, 1) - margin outputBoundaries(:, 2) + margin];


subplot(3, 1, 1); hold on;
plot(time_vec,Output(1,:));
plot(time_vec, xHat(1, :));
grid minor
ylim(ylims(1, :))
ylabel('Concentration')
legend({'x', '$\hat{x}$'}, 'Interpreter', 'Latex');

subplot(3, 1, 2); hold on;
plot(time_vec,Output(2,:));
plot(time_vec,xHat(2,:));
grid minor
ylim(ylims(2, :))
ylabel('Temperature')

subplot(3, 1, 3); hold on;
plot(time_vec,Output(3,:));
plot(time_vec,xHat(3,:));
grid minor
ylim(ylims(3, :))
ylabel('Tank level')

xlabel('time (m)')
hold off

figure(2)
subplot(2, 1, 1);
plot(time_vec,Input(1,:));
ylabel('Coolant Temperature')
grid minor
subplot(2, 1, 2);
plot(time_vec,Input(2,:));
ylabel('Outlet flowrate')
grid minor
xlabel('time (m)')
hold off

figure(3)
hold on
plot(time_vec,d)
plot(time_vec,Disturbance(1,:));
plot(time_vec,Disturbance(2,:));


if nd == 3
    plot(time_vec,Disturbance(3,:));
    legend({'$d$', '$\hat{d_1}$','$\hat{d_2}$','$\hat{d_3}$'}, 'Interpreter', 'Latex')
else
    legend({'$d$', '$\hat{d_1}$','$\hat{d_2}$', 'Interpreter'}, 'Interpreter', 'Latex')
end

xlabel('time (m)')
hold off