clc;
clear;
% close all;
%%
T0 = 0  ;
Ts = 0.1;
Tf = 20 ;
t = T0:Ts:Tf ; t = t';
% system = tf([0 0 0 1 -0.5] , [1 -0.2] , Ts , 'variable' , 'z^-1'); % system = (s+1)/(s^2+s+2)
k = 0.5 ;
tau = 2 ;
teta = 0.5 ;
system = k* tf(1,[tau ,1 ] , 'ioDelay' , teta) ;
h = step(system , t);
h = h(2:end);
StepInfo = stepinfo(system) ;
STime = StepInfo.SettlingTime ;
N = floor(STime/Ts)+1;
N1 = floor(teta/Ts) +1  ; % Prediction Horizon
N2 = floor(5*tau/Ts) + N1  ;
m  = 4  ; % Control Horizon
G = zeros(N2 , m);
for i=1:m
    G(: , i) = [zeros(i-1 , 1) ; h(1:N2-i+1)]   ;
end
G = G(N1:N2 , :) ;
H = zeros(N2 , N);
for i = 1:N2
    for j = 1:N
        H(i ,j) = h(i+j) - h(j) ;
    end
end
H = H(N1:N2 , :);
%% Discrete
system_dis = c2d(system , Ts) ;
B = system_dis.num  ; B = cell2mat(B) ; B = B(2:end) ;
A = system_dis.den ;  A = cell2mat(A) ;
DegSys = numel(A)-1 ;
%% Main Loop
Nt = numel(t) ;
y = zeros(Nt , 1) ;
u = zeros(Nt , 1) ;
dU = zeros(m , 1) ;
Umin = zeros(N , 1);
Fval = zeros(Nt , 1) ;
W = [ones(floor(Nt/4) , 1) ; 2*ones(floor(Nt/4) , 1) ; -ones(floor(Nt/4) , 1) ; zeros(floor(Nt/4) , 1)];
dist = 0.1.*(t>6).*(t<=6.5) - 0.1.*(t>6.5).*(t<=7);
DIST = 0 ;  % 1: add disturbance
            % 0: Don't Add
M = (5 * tau + teta)/Ts ;
f = M/500*(3.5 * tau/Ts+2-(M-1)/2) ;
Lambda = f * k^2 ; 
for i = 7:Nt-1
    y(i) = 0.9512 * y(i-1) + 0.02439 * u(i-6) ;
    for j = 1:N
        if i-j < 2
            Umin(j ,1) = 0 ;
        else
            Umin(j , 1) = u(i-j)-u(i-j-1) ;
        end
    end
    FreeResponse = H *Umin + y(i) +DIST *  dist(i);
    Uplus = (G' * G + Lambda)\(G' * (W(i) - FreeResponse)) ;
    %     Cost = @(Uplus)(W(i) - G*Uplus - FreeResponse)'*Q*(W(i) - G*Uplus - FreeResponse) + Uplus'*R*Uplus ;
    %     Cost = @(Uplus) norm(W(i) - G*Uplus - FreeResponse , 1) + Uplus'*R*Uplus ;
    %     [Uplus ,Fval(i)] = fmincon(Cost , randn(m,1)) ;
    dU = Uplus ;
    u(i) = u(i-1) + dU(1) ;
end
%% plot Results
figure(1) ;
subplot(2 , 1 ,1) ;
plot(t(1:end-1) , y(1:end-1) , t(1:end-1) , W(1:Nt-1),  'LineWidth' , 2) ; hold on
xlabel('Time (second)') ;
ylabel('Y Amp') ;
title('DMC Algorithm') ;
legend('Output' , 'Ref') ;
grid on

figure(1)
subplot(2,1,2) ;
plot(t(1:end-1) , u(1:end-1) , 'LineWidth' , 1.5) ; hold on
xlabel('Time (second)') ;
ylabel('U amp') ;
legend('U') ;
title('Input Signal (Effort Control)') ;
grid on

% figure(2)
% plot(t(1:end-1) , Fval(1:end-1) , 'LineWidth' , 1.5) ; hold on
% xlabel('Time (second)') ;
% ylabel('Cost Function amp') ;
% legend('Cost Function') ;
% title('Cost Function Variation') ;
% grid on



