%% System parameters and initial condition
h = 0.1;
X = [1;-1];

% Damped system
s = tf('s');
G = c2d(ss(1/(s^2 + 2*s + 1)),h);
A = G.A;
B = G.B;

%% Integrator system dynamics (Exercise 1)
A = [1,h;0,1];
B = [h^2/2; h];

%% Reference trajectory
xr = @(t) [sin(2*t); 2*cos(2*t)]; % reference [position, velocity]

%% MPC definition
Hp = 20; Hu = Hp-1;
xmax = 10; umax = 4;
%Q=[100,0;0,0];   R = 0; %(For damped system)
Q=[100,0;0,1]; R = 0; %(For integrator)

%% Define the H matrix (Exercise 2)
H = ?;

%% Define the Aineq and Bineq matrices for the domains (Exercise 3)
Aineq = ?;
Bineq = ?;

%% Define the Aeq and Beq matrices for the dynamics (Exercise 3)
Aeq = ?;
Beq = ?;

%% Set up the plotting
figure(1);
subplot(3,1,1); hold on; horizonPlot(1) = plot(0, 0,   'rx');
xlabel('Time $t$', 'Interpreter', 'latex'); ylabel('Position ${x}(t)$', 'Interpreter', 'latex')
subplot(3,1,2); hold on; horizonPlot(2) = plot(0, 0,   'bx');
xlabel('Time $t$', 'Interpreter', 'latex'); ylabel('Velocity $\dot{x}(t)$', 'Interpreter', 'latex')
subplot(3,1,3); hold on; horizonPlot(3) = plot(0, 0,   'gx');
xlabel('Time $t$', 'Interpreter', 'latex'); ylabel('Control $u$', 'Interpreter', 'latex')
opts = optimoptions(@quadprog); opts.Display = 'none';

%% Run the discrete-time MPC controlled system
for k = 0:ceil(20/h)
    %% Evaluate reference trajectory
    t = k*h;
    horizon = t + (0:h:(Hp-1)*h);
    xreval = reshape(xr(horizon), [2*Hp,1]);
    xrbar  = [xreval; zeros(Hu,1)];

    %% Compute f vector for the cost function definition (Ecercise 2)
    f = ?;
    
    %% Constrain the first state to start at x0 by appending terms to the equality constrain matrices (Exercise 3)
    Aeq = [Aeq; ?];
    Beq = [Aeq; ?];

    %% Compute control signal
    x = quadprog(H, f, Aineq, Bineq, Aeq, Beq, [], [], [], opts);
    U = x(2*Hp+1);
    
    %% Visualize MPC problem
    subplot(3,1,1); hold on; plot(t, X(1), 'ro');
    plot(horizon, xrbar(1:2:2*Hp),  'r.'); set(horizonPlot(1), 'XData', horizon, 'YData', x(1:2:2*Hp))
    if k==0; legend({'Prediction horizon of $x(t)$', 'System position $x(t=hk)$', 'Reference position $x_r(t)$'},'AutoUpdate','off', 'Interpreter', 'latex', 'Location', 'EastOutside'); end
    subplot(3,1,2); hold on; plot(t, X(2), 'bo');
    plot(horizon, xrbar(2:2:2*Hp+1),'b.'); set(horizonPlot(2), 'XData', horizon, 'YData', x(2:2:2*Hp+1))
    if k==0; legend({'Prediction horizon of $\dot{x}(t)$', 'System velocity $\dot{x}(t=hk)$', 'Reference velocity $x_r(t)$'},'AutoUpdate','off', 'Interpreter', 'latex', 'Location', 'EastOutside'); end
    subplot(3,1,3); hold on; plot(t, U, 'go');
    set(horizonPlot(3), 'XData', horizon(1:Hu), 'YData', x(2*Hp+1:end))
    if k==0; legend({'Control horizon of $u(t)$', 'System acceleration $u(t=hk)$'},'AutoUpdate','off', 'Interpreter', 'latex', 'Location', 'EastOutside'); end
    pause(0.000001)
    
    %% Update the state using system dynamics
    X = A*X + B*U;
end
