%% Geometrical meaning of Lie derivative
%  
%  File: lie_geometrical_meaning.m
%  Directory: 4_gyujtemegy/11_CCS/_2_nonlin-pannon/2018
%  Author: Peter Polcz (ppolcz@gmail.com) 
%  
%  Created on 2018. July 29.
%
%% 
% Requirements
%
% <script 2_demonstrations/lib/matlab/vekanal_subsmesh.m>, 
% <script 2_demonstrations/lib/matlab/vekanal_quiver_sym.m> and
% <script 2_demonstrations/lib/matlab/pcz_arrow.m>

syms t x1 x2 real

x = [ x1 ; x2 ];

Lie = @(f,h) jacobian(h,x) * f;

f = [
    x2 + 0.4*x2*x1
    -x1-x2 + 0.3*x1^2
    ];

h = sin(x1+x2^2)^2 + x2^2;

Lfh = Lie(f,h);
Lfh_fh = matlabFunction(Lfh,'vars',{x'});

%%%
% $$L_f h(x) =
% 2\,\cos\left({x_{2}}^2+x_{1}\right)\,\sin\left({x_{2}}^2+x_{1}\right)
% \,\left(x_{2}+\frac{2\,x_{1}\,x_{2}}{5}\right)-\left(2\,x_{2}+4\,x_{2}
% \,\cos\left({x_{2}}^2+x_{1}\right)\,\sin\left({x_{2}}^2+x_{1}\right)\right)
% \,\left(-\frac{3\,{x_{1}}^2}{10}+x_{1}+x_{2}\right)$$

%% <EXTRACT-THIS>
[x1_num,x2_num] = meshgrid(linspace(-0.5,1.5,31), linspace(-1,1,31));

h_fh = matlabFunction(h, 'vars', {x'});
h_num = vekanal_subsmesh(h, x, {x1_num, x2_num});


f_ode = matlabFunction(f, 'vars', {t x});
[tt, xx] = ode45(f_ode, 0:0.1:5, [0.8;0.8]);
xx1 = xx(:,1);
xx2 = xx(:,2);

N = numel(xx1);

I = [
    1:N-1
    1:N-1
    2:N
    2:N
    ];

h_xx = h_fh(xx);

figure('Position', [ 362 , 347 , 1442 , 535 ], 'Color', [1 1 1]), 

subplot(122), hold on
plot(tt, h_xx,'r','LineWidth',5), grid on;
plot(tt(1), h_xx(1),'r.','MarkerSize',45)
plot(tt, Lfh_fh(xx))
xlabel 'time (t)'
ylabel 'h(x(t))'
title 'Value of $h(x(t))$ along a system trajectory $x(t)$ and its derivative' interpreter latex

subplot(121), hold on
Surf = surf(x1_num, x2_num, h_num);
Surf.FaceAlpha = 0.5;

plot(xx1,xx2,'b','LineWidth',5);
plot(xx1(1),xx2(1),'b.','MarkerSize',45)
plot3(xx1,xx2,h_xx,'r','LineWidth',5);
plot3(xx1(1),xx2(1),h_xx(1),'r.','MarkerSize',45)

Z = h_xx(I);
Z([1,end],:) = 0;
Fill = fill3(xx1(I),xx2(I),Z,'red','FaceAlpha', 0.5, 'EdgeColor', 'none');

view([ -108.4 , 48.4 ]), grid on
xlabel 'x_1'
ylabel 'x_2'
zlabel 'h(x)'

title 'Value of $h(x)$ along a system trajectory $x(t)$ of $\dot x = f(x)$' interpreter latex

%% 
% The blue line is a system trajectory of $\dot x = f(x)$ with the initial
% condition illustrated by the blue dot. The red line highlights the value
% of function $h(x)$ along the trajectory $x(t)$. In this way $h(x(t))$ can
% be considered as a scalar function of $t$. Its time derivative can be
% computed by the chain rule: $\frac{\mathrm{d}}{\mathrm{d} t} h(x(t)) =
% \frac{\partial h}{\partial x} \cdot \dot x = \frac{\partial h}{\partial
% x} f(x)$.

%% Geometrical meaning of the Lie bracket
%% Local basis vectors in polar coordinates as two vector fields

syms x y real

Z = [x;y];

f_sym = [
    -y
    x
    ];

g_sym = [
    x
    y
    ] / sqrt(x^2 + y^2);

figure, hold on, grid on
Q1 = vekanal_quiver_sym(f_sym, Z, {0,8,9}, {-4,4,9}, 0.7);
Q2 = vekanal_quiver_sym(g_sym, Z, {0,8,9}, {-4,4,9}, 0.7);
title 'Local basis vectors in the polar coordinates'

%% <EXTRACT-THIS>

br_fg = simplify(jacobian(g_sym,Z)*f_sym - jacobian(f_sym,Z)*g_sym)

f_fh = matlabFunction(f_sym,'vars',{'t' Z});
g_fh = matlabFunction(g_sym,'vars',{'t' Z});

T1 = [0.4 0.8 1.4];
T2 = [1 3 5];

phi0 = [
    -10
    -20
    -40
    ] * pi / 180;

r0 = [
    3
    2
    1
    ];

x0 = round([ r0.*cos(phi0) r0.*sin(phi0) ],2);

hold on
for i = 1:numel(T1)

    fprintf('Path %d starting from from (%g,%g):\n', i, x0(i,:))
    fprintf(' -   %g seconds along the flow of f(x,y)\n', T1(i))
    fprintf(' -   %g seconds along the flow of g(x,y)\n', T2(i))
    fprintf(' -   %g seconds along the flow of f(x,y) (i.e. moving backwards)\n', -T1(i))
    fprintf(' -   %g seconds along the flow of g(x,y) (i.e. moving backwards)\n\n', -T2(i))

    N = 1000;
    [~,p1] = ode45(f_fh, linspace(0,T1(i),N), x0(i,:)');
    [~,p2] = ode45(g_fh, linspace(0,T2(i),N), p1(end,:)');
    [~,p3] = ode45(@(t,x) -f_fh(-t,x), linspace(0,T1(i),N), p2(end,:)');
    [~,p4] = ode45(@(t,x) -g_fh(-t,x), linspace(0,T2(i),N), p3(end,:)');

    plot(p1(:,1),p1(:,2), 'Color', Q1.Color, 'LineWidth', 5),
    plot(p2(:,1),p2(:,2), 'Color', Q2.Color, 'LineWidth', 5),
    plot(p3(:,1),p3(:,2), 'Color', Q1.Color, 'LineWidth', 5),
    plot(p4(:,1),p4(:,2), 'Color', Q2.Color, 'LineWidth', 5),

    plot(x0(i,1),x0(i,2),'ko', 'MarkerSize',10);

    pcz_arrow(p1(end/2,1),p1(end/2,2),p1(end/2+1,1),p1(end/2+1,2),'HeadLength',14,'HeadWidth',14)
    pcz_arrow(p2(end/2,1),p2(end/2,2),p2(end/2+1,1),p2(end/2+1,2),'HeadLength',14,'HeadWidth',14)
    pcz_arrow(p3(end/2,1),p3(end/2,2),p3(end/2+1,1),p3(end/2+1,2),'HeadLength',14,'HeadWidth',14)
    pcz_arrow(p4(end/2,1),p4(end/2,2),p4(end/2+1,1),p4(end/2+1,2),'HeadLength',14,'HeadWidth',14)
end
axis equal
axis tight

%% 
% A walk along the flow of f and g and then backwards is closed.

%% What if the Lie bracket is non-zero

syms x y real

Z = [x;y];

f_sym = [
    -y
    x^3
    ];

g_sym = [
    x
    y
    ] / sqrt(x^2 + y^2);

br_fg = simplify(jacobian(g_sym,Z)*f_sym - jacobian(f_sym,Z)*g_sym);

%% <EXTRACT-THIS>

figure('Position', [ 368 , 373 , 1448 , 455 ], 'Color', [1 1 1]), 

subplot(121), hold on, grid on
Q1 = vekanal_quiver_sym(f_sym, Z, {0,8,9}, {-4,4,9}, 0.7);
Q2 = vekanal_quiver_sym(g_sym, Z, {0,8,9}, {-4,4,9}, 0.7);
title 'The two vector fields'

subplot(122), hold on, grid on
Q3 = vekanal_quiver_sym(br_fg, Z, {0,8,9}, {-4,4,9}, 0.7, 'Color', [ 0.929 , 0.694 , 0.125 ]);
title 'Lie bracket of f and g'

%% 
% The walk along the flow of f and g is not closed
%% <EXTRACT-THIS>

f_fh = matlabFunction(f_sym,'vars',{'t' Z});
g_fh = matlabFunction(g_sym,'vars',{'t' Z});
br_fg_fh = matlabFunction(br_fg,'vars',{'t' Z});

T1 = [0.05 ];
T2 = [2 3 1];
T3 = [0.138 3 1];

phi0 = [
    -10
    -20
    -40
    ] * pi / 180;

r0 = [
    3
    2
    1
    ];

x0 = round([ r0.*cos(phi0) r0.*sin(phi0) ],2);

for i = 1:numel(T1)

    fprintf('Path %d starting from from (%g,%g):\n', i, x0(i,:))
    fprintf(' -   %g seconds along the flow of f(x,y)\n', T1(i))
    fprintf(' -   %g seconds along the flow of g(x,y)\n', T2(i))
    fprintf(' -   %g seconds along the flow of f(x,y) (i.e. moving backwards)\n', -T1(i))
    fprintf(' -   %g seconds along the flow of g(x,y) (i.e. moving backwards)\n\n', -T2(i))

    N = 1000;
    [~,p1] = ode45(f_fh, linspace(0,T1(i),N), x0(i,:)');
    [~,p2] = ode45(g_fh, linspace(0,T2(i),N), p1(end,:)');
    [~,p3] = ode45(@(t,x) -f_fh(-t,x), linspace(0,T1(i),N), p2(end,:)');
    [~,p4] = ode45(@(t,x) -g_fh(-t,x), linspace(0,T2(i),N), p3(end,:)');
    [~,p5] = ode45(@(t,x) br_fg_fh(t,x), linspace(0,T3(i),N), x0(i,:)');

    subplot(121), hold on
    plot(p1(:,1),p1(:,2), 'Color', Q1.Color, 'LineWidth', 5),
    plot(p2(:,1),p2(:,2), 'Color', Q2.Color, 'LineWidth', 5),
    plot(p3(:,1),p3(:,2), 'Color', Q1.Color, 'LineWidth', 5),
    plot(p4(:,1),p4(:,2), 'Color', Q2.Color, 'LineWidth', 5),

    plot(x0(i,1),x0(i,2),'ko', 'MarkerSize',10);

    pcz_arrow(p1(end/2,1),p1(end/2,2),p1(end/2+1,1),p1(end/2+1,2),'HeadLength',14,'HeadWidth',14)
    pcz_arrow(p2(end/2,1),p2(end/2,2),p2(end/2+1,1),p2(end/2+1,2),'HeadLength',14,'HeadWidth',14)
    pcz_arrow(p3(end/2,1),p3(end/2,2),p3(end/2+1,1),p3(end/2+1,2),'HeadLength',14,'HeadWidth',14)
    pcz_arrow(p4(end/2,1),p4(end/2,2),p4(end/2+1,1),p4(end/2+1,2),'HeadLength',14,'HeadWidth',14)

    subplot(122), hold on
    plot(p1(:,1),p1(:,2), 'Color', Q1.Color, 'LineWidth', 5),
    plot(p2(:,1),p2(:,2), 'Color', Q2.Color, 'LineWidth', 5),
    plot(p3(:,1),p3(:,2), 'Color', Q1.Color, 'LineWidth', 5),
    plot(p4(:,1),p4(:,2), 'Color', Q2.Color, 'LineWidth', 5),
    plot(p5(:,1),p5(:,2), 'Color', Q3.Color, 'LineWidth', 5),

    plot(x0(i,1),x0(i,2),'ko', 'MarkerSize',10);

    pcz_arrow(p1(end/2,1),p1(end/2,2),p1(end/2+1,1),p1(end/2+1,2),'HeadLength',14,'HeadWidth',14)
    pcz_arrow(p2(end/2,1),p2(end/2,2),p2(end/2+1,1),p2(end/2+1,2),'HeadLength',14,'HeadWidth',14)
    pcz_arrow(p3(end/2,1),p3(end/2,2),p3(end/2+1,1),p3(end/2+1,2),'HeadLength',14,'HeadWidth',14)
    pcz_arrow(p4(end/2,1),p4(end/2,2),p4(end/2+1,1),p4(end/2+1,2),'HeadLength',14,'HeadWidth',14)
    pcz_arrow(p5(end/2,1),p5(end/2,2),p5(end/2+1,1),p5(end/2+1,2),'HeadLength',14,'HeadWidth',14)
end
axis equal
axis tight
