clc;
clear;
close all;
%% 1: System Model
nfull = 16; 
N     = 8; 
[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_first = 0*ones(nx,1);
x_measuring = x_first;
x_measuringP2 = x_first;
x_measuringP3 = x_first;

% ditrubances
d0 = [0.5;1];
alpha = 0.1;
% 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*d{k} ];
    objective_function = objective_function + objective_function_k;
    constraints = constraints + constraints_k;
end

X = [x_measuring'];
XP2 = [x_measuringP2'];
XP3 = [x_measuringP3'];
U = [];
u1 = [];
u2 = [];
u1P2 = [];
u2P2 = [];
u1P3 = [];
u2P3 = [];


%% Solving the MPC problem
Options = sdpsettings('verbose', 0);
for i = 1:70
    
    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});

%   comparison P1 & P2
    Up1 = Uopt;
    up1 = u0opt;
    
    [Up2,up2] = problem2(N,Q,R,x_measuringP2,d0,alpha);
    [Up3,up3] = problem3(N,Q,R,x_measuringP3,d0,alpha);

    Up1;
    Up2;
    Up3;
    % new states for problems P1 & P2 & P3
    x_measuring = A*x_measuring + B*u0opt + Bd*d0;
    x_measuringP2 = A*x_measuringP2 + B*up2 + Bd*d0;
    x_measuringP3 = A*x_measuringP3 + B*up3 + Bd*d0;
    d0 = alpha*d0;
    
    
    X = [X; x_measuring'];
    XP2 = [XP2; x_measuringP2'];
    XP3 = [XP3; x_measuringP3'];
    U = [U; u0opt];
    u1 = [u1; u0opt(1)];
    u2 = [u2; u0opt(2)];
    u1P2 = [u1P2; up2(1)];
    u2P2 = [u2P2; up2(2)];
    u1P3 = [u1P3; up3(1)];
    u2P3 = [u2P3; up3(2)];

    
end
Y = C*X';
YP2 = C*XP2';
YP3 = C*XP3';
%% Plots
Tx = 0:size(X, 1)-1;
Tu = 0:size(u1, 1)-1;
Ty = 0:size(Y,2)-1;

%% Plot P1
% figure;
% % plot (states, inputs)
% subplot(2,1,1)
% stairs(Tx*Ts, X);
% % plot(Tx, X);
% xlabel('Time [sec]');
% ylabel('States');
% title('Problem 1');
% subplot(2,1,2)
% stairs(Tu*Ts, u1);
% hold on;
% stairs(Tu*Ts, u2,'g');
% % plot(Tu, U);
% xlabel('Time [sec]');
% ylabel('Inputs');
% title('inputs');

% plot (outputs, inputs)
figure;
subplot(2,1,1)
stairs(Ty*Ts, Y');
% plot(Ty*Ts, Y);
xlabel('Time [sec]');
ylabel('Outputs');
title('Problem 1');
subplot(2,1,2)
stairs(Tu*Ts, u1);
hold on;
stairs(Tu*Ts, u2,'g');
% plot(Tu, U);
xlabel('Time [sec]');
ylabel('Inputs');
title('inputs');


%% Plot P2
% figure;
% % plot (states, inputs)
% subplot(2,1,1)
% stairs(Tx*Ts, XP2);
% % plot(Tx, X);
% xlabel('Time [sec]');
% ylabel('Outputs');
% title('Problem 2');
% subplot(2,1,2)
% stairs(Tu*Ts, u1P2);
% hold on;
% stairs(Tu*Ts, u2P2,'g');
% % plot(Tu, U);
% xlabel('Time [sec]');
% ylabel('Inputs');
% title('inputs P2');

% plot (outputs, inputs)
figure;
subplot(2,1,1)
stairs(Ty*Ts, YP2');
% plot(Ty, YP2);
xlabel('Time [sec]');
ylabel('Outputs');
title('Problem 2');
subplot(2,1,2)
stairs(Tu*Ts, u1);
hold on;
stairs(Tu*Ts, u2,'g');
% plot(Tu, U);
xlabel('Time [sec]');
ylabel('Inputs');
title('inputs');

%% Plot P3
% figure;
% subplot(2,1,1)
% stairs(Tx*Ts, XP3);
% % plot(Tx, X);
% xlabel('Time [sec]');
% ylabel('Outputs');
% title('Problem 3');
% subplot(2,1,2)
% stairs(Tu*Ts, u1P3);
% hold on;
% stairs(Tu*Ts, u2P3,'g');
% % plot(Tu, U);
% xlabel('Time [sec]');
% ylabel('Inputs');
% title('inputs P3');

% plot (outputs, inputs)
figure;
subplot(2,1,1)
stairs(Ty*Ts, YP3');
% plot(Ty, YP3);
xlabel('Time [sec]');
ylabel('Outputs');
title('Problem 3');
subplot(2,1,2)
stairs(Tu*Ts, u1);
hold on;
stairs(Tu*Ts, u2,'g');
% plot(Tu, U);
xlabel('Time [sec]');
ylabel('Inputs');
title('inputs');


