%% Algebraická smyčka a DAE

%%
% Uvažujme zpětnovazební zapojení dvou lineárních časově invariantních
% stavových modelů. Tyto modely jsou prvního řádu a popsány parametry 
% (a1,b1,c1,d1) a (a2,b2,c2,d2) s klasickou interpretaci. Propojeny jsou
% jako na screenshotu ze Simulinku
% 
% <<screenshot_simulink_algebraic_loop.png>> 
%
% V závislosti na hodnotách členů d1 a d2 se výsledný model chová jako DAE
% indexu 1 nebo 2.

%% Sestavení výsledného DAE modelu v Matlabu
% Dvě matice, které definují výsledný DAE model $E\dot x(t) = A x(t)$ jsou

E = [1 0 0 0; 0 1 0 0; 0 0 0 0; 0 0 0 0]

a1 = -4; b1 = 1/3; c1 = 1/2; d1 = 1/3;
a2 = -4; b2 = 1/3; c2 = 1/2; d2 = 3;
A = [a1 0 0 b1; 0 a2 b2 0; c1 0 -1 d1; 0 c2 d2 -1]

%%
% Jsou to vlastně dvě diferenciální rovnice popisující ty dva původní
% systémy prvního řádu, a pak dvě algebraické (lineární) rovnice popisující
% jejich propojení.

%% DAE model v Matlabu
% Chceme-li ta data (matice A a E) použít k tvorbě DSS objektu v Matlabu,
% musíme nejdříve (svévolně) určit i matice B, C a D. 

Gdss = dss(A,[1;0;0;0],eye(4,4),zeros(4,1),E)

%% Převod na stavový model
% Nejdříve strukturujeme tu velkou matici A pro DAE model na bloky
A11 = A(1:2,1:2);
A12 = A(1:2,3:end);
A21 = A(3:end,1:2);
A22 = A(3:end,3:end)

%%
% a vytvoříme matici odpovídající stavovému modelu
As = A11 - A12*(A22\A21)
Gs = ss(As,zeros(2,1),eye(2,2),0);


%% Nastavení počátečních podmínek pro simulaci
t0 = 0;
t1 = 1.6;

x10 = 1;
x20 = 2;
x0 = [x10; x20];

y0 = -A22\(A21*x0)
y10 = y0(1);
y20 = y0(2);
y0 = [1;1];

%% Simulace redukovaného stavového modelu
figure(1)
initial(Gs,x0)

% figure(2)
% initial(Gdss,[x0;y0],t1) % does not work anyway

%% Řešení DAE pomocí obecného solveru
M = [1 0 0 0; 0 1 0 0; 0 0 0 0; 0 0 0 0];
options = odeset('Mass',M,'RelTol',1e-4,'AbsTol',1e-6);

%%
tspan = [t0 t1];
[t,x] = ode15s(@(t,x) DAE_feedback(t,x,A),tspan,[x0;y0],options);

%%
figure(3)
plot(t,x)
legend({'x1','x2','y1','y2'})

%% Ověření konzistence počátečních podmínek
%
A21*x(1,1:2)'+A22*x(1,3:4)'

%% Řešení numericky voláním Simulinku
simout = sim('algebraicka_smycka_a_DAE_v_simulinku')

%%
%
figure(4)
plot(simout.logsout{1}.Values)
hold on
plot(simout.logsout{2}.Values)
plot(simout.logsout{3}.Values) 
plot(simout.logsout{4}.Values), legend({'x1','x1','y2','y1'})
hold off

%% Ověření konzistence počátečních podmínek
%
A21*[simout.logsout{1}.Values.Data(1);simout.logsout{2}.Values.Data(1)]+A22*[simout.logsout{4}.Values.Data(1);simout.logsout{3}.Values.Data(1)]


%%
function out = DAE_feedback(t,x,A) 
    out = A*x;
end