%% Embedding
%  
%  File: embedding.m
%  Directory: 4_gyujtemegy/11_CCS/_2_nonlin-pannon/2018
%  Author: Peter Polcz (ppolcz@gmail.com) 
%  
%  Created on 2018. July 23.
%

%% Embedding into QP

syms t x1 x2 p1 p2 p3 real

x = [x1 ; x2];
p = [p1 ; p2 ; p3];

f_sym = [
    -1 / (x1^2 + 2*x2 + 3)
    1 / (x2 + x1)
    ];

g_sym = [
    2
    -1 / x2
    ];

h_sym = 1 / x2;

%% 

pcz_latex_output(f_sym, 'label', 'f(x) = ')
pcz_latex_output(g_sym, 'label', 'g(x) = ')
pcz_latex_output(h_sym, 'label', 'h(x) = ')

%%

p_def = [
    1/x2
    1/(x1+x2)
    1/(x1^2 + 2*x2 + 3)
    ];

fp_sym = subs(f_sym, p_def, p);
gp_sym = subs(g_sym, p_def, p);

F_sym = [
    fp_sym
    subs(jacobian(p_def, x)*fp_sym, p_def, p)
    ];

G_sym = [
    gp_sym
    subs(jacobian(p_def, x)*gp_sym, p_def, p)
    ];

H_sym = subs(h_sym, p_def, p);

%% 

pcz_latex_output(F_sym, 'label', 'f(x,p) = ')
pcz_latex_output(G_sym, 'label', 'g(x,p) = ')
pcz_latex_output(H_sym, 'label', 'h(x,p) = ')

%% Simulate the rational model (before embedding)

u0 = 1;
x0 = [
    0
    -1.15
    ];

f_fh = matlabFunction(f_sym + u0*g_sym, 'vars', {t x});

figure('Position', [ 605 , 351 , 486 , 364 ], 'Color', [1 1 1])
[tt,xx] = ode45(f_fh, [0,10], x0);
plot(tt,xx(:,[1 2])), grid on;
title 'Simulation of the original system ($x_1$, $x_2$)' interpreter latex


%% Simulate the polynomial system (after embedding)

z = [x;p];
z_def = subs(z,p,p_def);
z_fh = matlabFunction(z_def, 'vars', {x});

z0 = z_fh(x0);

F_fh = matlabFunction(F_sym + u0*G_sym, 'vars', {t z});
[tt,xx] = ode45(F_fh, [0,10], z0);

figure('Position', [ 529 , 351 , 1124 , 355 ], 'Color', [1 1 1])

subplot(121)
plot(tt,xx(:,[1 2])), grid on;
title 'Simulation of the embedding system ($x_1$, $x_2$)' interpreter latex

subplot(122)
plot(tt,xx(:,[3 4 5])), grid on;
title 'Simulation of the embedding system ($p_1$, $p_2$, $p_3$)' interpreter latex
