clc;
clear;
close all;
%%
Ts = 0.1 ; 
Tf = 10 ;
t = 0:Ts:Tf ; 
N = numel(t) ;
y = zeros(1 , N) ; y(1) = 0.5 ;
u = zeros(1 , N) ; 


Yref = ones(1 , N); Yref(t>3) = 2 ; Yref(t>7) = 0.5 ;
LB = 30*[-1;-1] ;
UB = 30*[1;1] ;

for i = 1:N
   cost = @(U) Cost(y(i) , Yref(i) , U)  ;
   U0 = randn(2,1);
   [Uopt , Fval] = fmincon(cost , U0,[],[],[],[],LB , UB) ; 
   u(i) = Uopt(1) ;
   y(i+1) = 0.9*y(i)-0.1*y(i)^3+0.05*u(i) ; 
end
y = y(1:N) ;
%% plots

figure(1) ; 
plot(t , Yref , t ,  y , 'LineWidth' , 2) ; 
grid on
xlabel('Time (Seconds)') ;
ylabel('Y') ;
title('MPC Controller') ;
legend('Yref','Y');

figure(2) ; 
plot(t , u , 'LineWidth' , 2) ; 
grid on
xlabel('Time (Seconds)') ;
ylabel('u') ;
title('MPC Controller') ;
legend('u');