function [u, x, y] = hw3_cvutID()
% Continous state-space description
Ac = [-.0151 -60.5651 0 -32.174;
     -.0001 -1.3411 .9929 0;
     .00018 43.2541 -.86939 0;
      0      0       1      0];

%  Inputs: elevator angle [deg] and flaperon angle [deg]
Bc = [-2.516 -13.136;
     -.1689 -.2514;
     -17.251 -1.5766;
     0        0];
% Outputs: attack angle [deg] and pitch angle [deg]
Cc = [0 1 0 0;
     0 0 0 1];
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 = 10; % Prediction horizon
Q = diag([10 10]); % tracking weight matrix
R = 0.1*eye(2); % input increment weight matrix

% Input constraints
umax = 25*[1 1]';
umin = -25*[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 = 75*Ts; % Final time
t = 0:Ts:Tf; % time vector

% Intial conditions
x0 = zeros(4,1); 
u0 = zeros(2,1); 
xtilde0 = [x0; u0];

% Generate the reference signal
ref = [2*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