
Fontsize = 20;
set(0,'DefaultAxesFontSize',Fontsize)

figure(1);
clf
subplot(2,2,1);
plot(time, state_sim(:,1),'-bo', 'linewidth', 2, 'MarkerSize', 8); hold on
plot(time(end-1)+Ts*[0:N], output.x(:,1),'--bx', 'linewidth', 1, 'MarkerSize', 8); 
xlabel('time(s)')
ylabel('x1')
% grid on;
ylim([-1 1])
xlim([0 time(end-1)+T])

subplot(2,2,2);
plot(time, state_sim(:,2), '-bo', 'linewidth', 2, 'MarkerSize', 8); hold on
plot(time(end-1)+Ts*[0:N], output.x(:,2), '--bx', 'linewidth', 1, 'MarkerSize', 8); 
xlabel('time(s)')
ylabel('x2')
% grid on;
ylim([-1 1])
xlim([0 time(end-1)+T])

subplot(2,2,3:4);
stairs(time(1:end-1), controls_MPC, '-ro', 'linewidth', 2, 'MarkerSize', 8); hold on
stairs(time(end-1)+Ts*[0:N-1], output.u, '--rx', 'linewidth', 1, 'MarkerSize', 8);
xlabel('time(s)')
ylabel('u')
% grid on;
ylim([-1 1])
xlim([0 time(end-1)+T])


if( nextTime == Te ) 
    D = 0.8;
    suptitle(['-- Disturbance to system state 2 of size: ' num2str(D) '! --'])
    state_sim(end,2) = state_sim(end,2) + D; 
elseif( nextTime == Te+Ts ) 
    suptitle(['-- Disturbance to system state 2 of size: ' num2str(D) '! --'])
else
    suptitle('Closed-loop NMPC simulation')
end
    