clc;
clear;
% close all;
%%
T0 = 0 ;
Ts = 0.1;
Tf = 100 ;
t = T0:Ts:Tf ; t = t' ;
system = tf([1 1],[1 1 2]) ;  % system = (s+1)/(s^2+s+2) ;
% impulse(system) ;
Info = stepinfo(system) ;
N = floor(Info.SettlingTime/Ts) + 10;
p = 10 ;   % Prediction  Horizon
m = 5  ;   % Control Horizon 
Q = 10 ;
R = 1  ;
system_dis = c2d(system , Ts) ; % y(k) = 1.886y(k-1) - 0.9048y(k-2) + 0.09968u(k-1) - 0.09017u(k-2)
% A =  [ 1 -1.886 0.9048] ;
% B = [0.09968 -0.09017] ;
A = system_dis.den ; A = cell2mat(A) ;
B = system_dis.num ; B = cell2mat(B) ; B = B(2:end) ;
h = impulse(system , t) ; h = h(2:end); % Delete h0
H1 = zeros( p ,m) ;
H2 = zeros( p , N-1) ;
for i = 1:m
    H1(: , i) = [zeros(i-1 , 1) ; h(1:p-i+1)] ;
end
for i = 1:m
    H2(i , :) = [zeros(i-1 , 1) ; h(N:-1:i+1)] ;
end
%% Main Loop
Nt = numel(t) ;
W = [ones(floor(Nt/4) , 1) ; 2*ones(floor(Nt/4) , 1) ; -1*ones(floor(Nt/4) , 1) ; zeros(floor(Nt/4) , 1)] ;
y = zeros(Nt , 1) ;
u = zeros(Nt , 1) ;
DegSys = numel(A) -1  ;
Uminus = zeros(N-1 , 1) ;
Fval = zeros(Nt , 1) ;
for i = 3:Nt-1
    y(i) = 1.886*y(i-1) - 0.9048*y(i-2) + 0.09968*u(i-1) - 0.09017*u(i-2) ;
    %  y(i) = -A(2:end)*y(i-1:-1:i-DegSys)' + B * u(i-1:-1:i-DegSys) ;
    for j = 1:N-1
        if i-N+j < 1
            Uminus(j , 1) = 0 ;
        else
            Uminus(j , 1) = u(i-N+j) ;
        end
    end
    FreeResponse = H2 * Uminus ;
    dUopt = (H1'*Q*H1 + R)\(H1' * Q * (W(i) - FreeResponse)) ;
%     Cost = @(Uopt)  norm(W(i) - H1*Uopt -FreeResponse , 2) + norm(Uopt , 2);
%     [dUopt , Fval(i)] = fminunc(Cost , randn(m,1)) ;
    u(i) = dUopt(1) ;
end
%% plot Results
figure(1) ;
subplot(2,1,1) ;
plot(t(1:Nt-1) , W(1:Nt-1) , t(1:Nt-1) , y(1:Nt-1) * W(20/Ts)/y(20/Ts) , 'LineWidth' , 2) ; hold on
xlabel('Time  (second)') ;
ylabel('Amp  y') ;
legend('Ref' , 'Out') ;
title(['OutPut Signal (Mac)  ,   Gain = ' ,  num2str(W(20/Ts)/y(20/Ts))]) ;
grid on

subplot(2,1,2) ;
plot( t(1:Nt-1) , u(1:Nt-1)*W(20/Ts)/y(20/Ts)  , 'LineWidth' , 2) ; hold on
xlabel('Time  (second)') ;
ylabel('Amp  u') ;
title('Effort Control Response (Mac)') ;
legend('U') ;
grid on

% figure(2)
% plot(t(1:Nt-1) , Fval(1:Nt-1) , 'r-' , 'LineWidth' , 2) ;
% xlabel('Time  (second)') ;
% ylabel('CF');
% grid on
% title('Cost Function Variation') ;
% legend('Cost') ;