cd ~/src/utb/ak9im/ak9im/p3;

%
%% load model

%load('data/task3.slx');
%load("data/u.csv");
out.u = load('data/u.csv')
out.y = load('data/y.csv')


step(1, [2 5 1])

%
%% run the simulation in simulink

len_u = length(out.u);
len_y = length(out.y);

% cd ~/src/utb/ak9im/ak9im/p2/data/;

m = [out.u, out.y];
% writematrix(m, 'data/m.csv')

plot(1:len_u,out.u)
plot(1:len_y,out.y)

len = min(len_u, len_y);

u = out.u(1:len)
stp = round(len_y / len)
y = out.y(1:stp:len_y) % navzorkujeme y na rovnaky pocet vzorku jako u
plot(1:len,y)

% zvolime okno o velkosti 3 a spocitame autoregresny model s exogennim vstupom (ARX)
% pomocou metody najmensich stvorcov
yy = y(3:len);
F = [-y(2:len-1) -y(1:len-2) u(2:len-1) u(1:len-2)];
% z = inv(F' * F) * F' * yy;
z = (F' * F) \ (F' * yy);
arx_B = [0 z(3) z(4)]
arx_A = [1 z(1) z(2)]

% obdobne spocitame rekurzivnu variantu
phi = zeros(4, len-3);
thm = zeros(4, len-3);
ehat = zeros(len-3, 1);
C = zeros(4, 4, len-3);
C(:,:,1) = 10^6 * eye(4);
C(:,:,2) = 10^6 * eye(4);
C(:,:,3) = 10^6 * eye(4);

for k = 3 : len
    phi(:, k) = [-y(k-1) -y(k-2) u(k-1) u(k-2)]';
    ehat(k) = y(k) - phi(:, k)' * thm(:, k-1);
    C(:, :, k) = C(:, :, k-1) - (C(:, :, k-1)*phi(:, k)*phi(:, k)'*C(:, :, k-1))/(1+phi(:, k)'*C(:, :, k-1)*phi(:, k));
    thm(:, k) = thm(:, k-1) + C(:, :, k)*phi(:, k)*ehat(k);
end
rarx_B = [0 thm(3, len) thm(4, len)]
rarx_A = [1 thm(1, len) thm(2, len)]
rarx_error = ehat(len)

plot(1:len, thm(1, :), 'blue')
hold on
plot(1:len, thm(2, :), 'red')
plot(1:len, thm(3, :), 'green')
plot(1:len, thm(4, :), 'black')
hold off
plot(1:len, ehat)

% analyticky vypocitane parametre diskretneho model zo spojiteho modelu
[n, d] = c2dm(1, [2 5 1], 1, 'zoh');
c2dm_B = n
c2dm_A = d

% pre porovnanie - Matlab funkcie arx a rarx
M = arx([y u], [2 5 1]);
z_matlab = M.Report.Parameters.ParVector;
arx_matlab_B = [0 z_matlab(3) z_matlab(4)]
arx_matlab_A = [1 z_matlab(1) z_matlab(2)]

[th, yh, P,  phi] = rarx([y u], [2 5 1], 'ff', 1.0);
rarx_matlab_B = [0 th(len, 3) th(len, 4)]
rarx_matlab_A = [1 th(len, 1) th(len, 2)]
rarx_m_err = (y - yh);
rarx_matlab_error = rarx_m_err(len)