% clc;
% clear;
%% 1: System Model
nfull = 16; 
N     = 8; 
alpha = 0.3;
[Gfull] = model_baltrunc(nfull);
% make a discrete-time approximation
Ts	= 1;
Gfull = c2d(Gfull,Ts);
% number of states
nx = size(Gfull.a, 2);
% number of inputs
nu = 2;
% number of outputs
ny = size(Gfull.c, 1);
% number of disturbances
nd = 2;
[A B C Bd] = deal(Gfull.a,Gfull.b(:,1:nu),Gfull.c,Gfull.b(:,nu+1:end));

%% Make the MPC controller
% weight matrices
Q = 1*(diag(ones(16,1),0));
R = diag(ones(2,1),0);
% initial conditions
x_measuring = zeros(nx,1);
% parameter, but now for example
d0 = [1;5];

% constraints
umax  = 1; umin = -1;

% definition of optimization variables
x = {};
u = {};
d = {};
for k = 0:N
    x{k+1} = sdpvar(nx, 1);
end

for k = 0:N-1
    u{k+1} = sdpvar(nu, 1);
end

for k = 1:N+1
    d{k} = sdpvar(nd, 1);
end

% objective function of MPC problem
objective_function = 0;
constraints = set([]);

% formulation of MPC problem
for k = 1:N
   objective_function_k = x{k}'*Q*x{k} + u{k}'*R*u{k};
    constraints_k = [ x{k+1} == A*x{k} + B*u{k} + Bd*d{k} ] + ...
                    [ umin <= u{k} <= umax ] + ...
                    [ d{k+1} == (alpha^k)*d{k} ];
    objective_function = objective_function + objective_function_k;
    constraints = constraints + constraints_k;
end

X = [x_measuring'];
U = [];
u1 = [];
u2 = [];

%% Solving the MPC problem
Options = sdpsettings('verbose', 0);
for i = 1:60
    fprintf('%d\r', i);
    
    sol = solvesdp(constraints + [x{1} == x_measuring, d{1} == d0], objective_function, Options);
    if sol.problem == 1
        disp('MPC problem is not solvable');
        break
    end

    % optimal value of U = [u(0), u(1), u(2), ..., u(N-1)]
    Uopt = double(cat(1,u{:}));

    % optimal value u(0)
    u0opt = double(u{1});

    % vypocet noveho stavu
    % x_measuring = A*x_measuring + B*u0opt + (alpha)^i*Bd*d0;
    x_measuring = A*x_measuring + B*u0opt + Bd*d0;
    d0 = (alpha^i)*d0;
    
    X = [X; x_measuring'];
    U = [U; u0opt];
    u1 = [u1; u0opt(1)];
    u2 = [u2; u0opt(2)];
    
   
end

%% Plots
Tx = 0:size(X, 1)-1;
Tu = 0:size(u1, 1)-1;

% close all;
figure;
subplot(2,1,1)
stairs(Tx*Ts, X);
% plot(Tx, X);
xlabel('t [sec]');
ylabel('X');
title('states');
subplot(2,1,2)
stairs(Tu*Ts, u1);
hold on;
stairs(Tu*Ts, u2,'g');

% figure;
% plot(Tu, U);
xlabel('t [sec]');
ylabel('U');
title('inputs');


