function [u, x, y] = hw3_cvutID()
% % Continous state-space description
Ac = [  -1.30533442596423 -0.664566611135402 -0.816231169912064 0.498259346200738 0.845747959447405;
        -0.664566611135401 -4.66190762531937 -2.46001902240756 -1.29448844337989 0.320444904254386;
        -0.816231169912064 -2.46001902240755 -2.80020522586735 -0.0470839007820536 0.939356292974667;
        0.498259346200739 -1.29448844337989 -0.047083900782053 -2.55869771397362 -1.19212564726104;
        0.845747959447406 0.320444904254385 0.939356292974667 -1.19212564726104 -1.84771111341511];
%  Inputs
Bc = [  0 0.205304683292191;
        -0.615507335128413 1.19293039852572;
        1.31415480877584 -0.802823098305938;
        -1.45506658248224 0;
        0 -0.149331353183561];
% Outputs
Cc = [-1.63644667825389 0.828387265991092 0 -0.302032298446366 0.914851752841074;
    0 0.217738348381478 -0.536821821287498 1.81358213090433 0];
    
% sys = rss(5, 2, 2);
% [Ac, Bc, Cc, Dc] = ssdata(sys);

Dc = [0 0;
     0 0];

sys=ss(Ac,Bc,Cc,Dc);

% Discretize the model
Ts = .05;     %Sampling time
model=c2d(sys,Ts);

% Extract the matrices of the discrete state-space description
A = model.A;
B = model.B;
C = model.C;

n = size(A,1); % number of states
m = size(B,2); % number of inputs
p = size(C,1); % number of outputs

%% MPC
N = 6; % Prediction horizon
Q = diag([10 10]); % tracking weight matrix
R = 0.1*eye(2); % input increment weight matrix

% Input constraints
umax = 7*[1 1]';
umin = -7*[1 1]';

% Augmented model
Atilde = [A B;
          zeros(m,n) eye(m)];
Btilde = [B;
          eye(m)];
Ctilde = [C zeros(p,m)];

% Your code goes here
H = [];
F = [];

G = [];
W = [];
S = [];

%% Simulation
Tf = 100*Ts; % Final time
t = 0:Ts:Tf; % time vector

% Intial conditions
x0 = zeros(n,1); 
u0 = zeros(p,1); 
xtilde0 = [x0; u0];

% Generate the reference signal
ref = [-4*ones(1, numel(t)+N); 10*ones(1, numel(t)+N)]';
ref(round(end/2):end, :) = ref(round(end/2):end, :)/2;

% Initialize the matrices where the simulation results will be stored
x = zeros(n, numel(t));
y = zeros(p, numel(t));
u = zeros(m, numel(t));

x(:,1) = x0;
y(:,1) = C*x0;
uprev = u0;

% Initialize OSQP (uncomment if OSQP is used)
% mosqp = osqp;
% settings = mosqp.default_settings();
% settings.verbose = 0;
% mosqp.setup(H/2+H'/2, [x(:,i)' uprev' vec(ref(1:N, :)')']*F, G, -inf*ones(size(W)), W+S*[x(:,i); uprev], settings);
for i = 1:(numel(t)-1)
    % Reference signal over the current prediction window
    rk = ref(i:i+N-1, :)';
    
    % Sove the QP
    % by quadprog from Optimization Toolbox ...
    qp_res = quadprog(H/2+H'/2, [x(:,i)' uprev' rk(:)']*F, G, W+S*[x(:,i); uprev], [], [], [], [], [], optimoptions('quadprog', 'Display', 'off'));

    % or by qpOASES ...
%     [qp_res,fval,exitflag,iter,lambda,auxOutput] = qpOASES(H/2+H'/2, F'*[x(:,i)' uprev' rk(:)']', G, [], [], [], W+S*[x(:,i); uprev]);

    % or by OSQP
%     mosqp.update('q', [x(:,i)' uprev' rk(:)']*F, 'u', W+S*[x(:,i); uprev]);
%     results = mosqp.solve();
%     qp_res = results.x;
    
    % Extract the incement in u
    duk = qp_res(1:m,1);
    
    u(:,i) = uprev + duk;
    
    % Simulation
    x(:,i+1) = A*x(:,i) + B*u(:,i);
    y(:,i+1) = C*x(:,i+1);
        
    uprev = u(:,i);
end

% Plot the results (just for your convenience)
figure(1)
subplot(211)
stairs(t, ref(1:numel(t),:), 'k--');
hold on
stairs(t, y');
hold off
title('Outputs')
xlabel('Time [s]')
ylabel('[deg]')
grid on
axis tight

subplot(212)
stairs(t, u');
title('Controls')
xlabel('Time [s]')
ylabel('[deg]')
grid on

end