clear;
xdel(winsid());
clc;

// czas regulacji
// tr = 

// parametry modelu obiektu
// k = 
// T1 = 
// T2 = 

// obliczenia pomocnicze

// nastawy regulatora
// kp = 
// Ti = 
// Td = 

PLOT_STEP = 10;
LAB_FS = 3;

if fileinfo('skok.txt') == [] then
    abort;
end
d = fscanfMat('skok.txt');
t = d(1:PLOT_STEP:$, 1)/1000;
y = d(1:PLOT_STEP:$, 2);
u = d(1:PLOT_STEP:$, 4);
w = d(1:PLOT_STEP:$, 8);

iStep = find(diff(w) <> 0);
iStart1 = 1;
tBefore = t(iStep(1));
if tBefore > 5 then
    iStart1 = find(t >= tBefore - 5, 1);
    tBefore = 5;
end
iStart2 = find(t > t(iStep(2)) - tBefore, 1);
len = min(iStep(2) - iStart1, length(t) - iStart2) - 1;
iEnd1 = iStart1 + len;
iEnd2 = iStart2 + len;
tr1 = t(iStart1:iEnd1);
tr1 = tr1 - t(iStep(1));
tr2 = t(iStart2:iEnd2);
tr2 = tr2 - t(iStep(2));
yr1 = y(iStart1:iEnd1);
yr2 = y(iStart2:iEnd2);
wr1 = w(iStart1:iEnd1);
wr2 = w(iStart2:iEnd2);
ur1 = u(iStart1:iEnd1);
ur2 = u(iStart2:iEnd2);

R = kp * (1 + 1 / Ti / %s + Td * %s / (Td * %s / 10 + 1));
G = k / ((T1 * %s + 1) * (T2 * %s + 1));
Gop = R * G;
Gcl = Gop / (1 + Gop);
ts = linspace(0, tr1($), 300);
dw1 = w(iStep(1) + 1) - w(iStep(1));
ys1 = dw1 * csim('step', ts, syslin('c', Gcl));

ys1 = ys1 + w(iStart1);
dw2 = w(iStep(2) + 1) - w(iStep(2));
ys2 = dw2 * csim('step', ts, syslin('c', Gcl));
ys2 = ys2 + w(iStart2);

subplot(2, 1, 1);
plot(tr1, [wr1 yr1]);
h = get('hdl');
h.children(2).foreground = 6;
plot(ts, ys1);
h = legend('wartość zadana', 'rejestracja', 'symulacja');
h.font_size=3;
title('Odpowiedź skokowa', 'fontsize', 3);
xlabel('t [s] - skala 1:300', 'fontsize', LAB_FS);
ylabel('glukoza [mg/dL]', 'fontsize', LAB_FS);
xgrid;
subplot(2, 1, 2);
plot(tr1, ur1);
h = get('hdl');
h.children(1).foreground = 5;
h = legend('sterowanie');
h.font_size=3;
xlabel('t [s] - skala 1:300', 'fontsize', LAB_FS);
ylabel('insulina [%]', 'fontsize', LAB_FS);
xgrid;

scf();
subplot(2, 1, 1);
plot(tr2, [wr2 yr2]);
h = get('hdl');
h.children(2).foreground = 6;
plot(ts, ys2);
h = legend('wartość zadana', 'rejestracja', 'symulacja', opt=4);
h.font_size=3;
title('Odpowiedź skokowa', 'fontsize', 3);
xlabel('t [s] - skala 1:300', 'fontsize', LAB_FS);
ylabel('glukoza [mg/dL]', 'fontsize', LAB_FS);
xgrid;
subplot(2, 1, 2);
plot(tr2, ur2);
h = get('hdl');
h.children(1).foreground = 5;
h = legend('sterowanie', opt=4);
h.font_size=3;
xlabel('t [s] - skala 1:300', 'fontsize', LAB_FS);
ylabel('insulina [%]', 'fontsize', LAB_FS);
xgrid;