%%
%  Author: Peter Polcz (ppolcz@gmail.com) 
%  Created on 2023. October 25. (2023a)
%

%{

pcz_num2str_multiline(A,b,'round',20,'format','%20.17f')

%}

A = [
    -0.50583713121586915 -0.85050158495257344
     0.85050158495508377  0.50583713120783091
    ];

b = [
     1.39780409270215111
    -1.39780409269080930
    ];

LimX = 3;
LimY = 3;

%%% 
% Generate anonymous functions
% 
x = sym('x',[2,1]);
f = diag(x)*(A*x+b);
f_fh = matlabFunction(f,'vars',{'t',x});
f_fh = @(t,x) diag(x)*(A*x+b);
J_fh = matlabFunction(jacobian(f,x),'vars',{x});
f1_fh = matlabFunction(f(1),'vars',x);
f2_fh = matlabFunction(f(2),'vars',x);

%%%
% Compute equilibria
%
Eq0 = [0;0];
Eq1 = -A\b;
EqX = [-b(1)/A(1,1) ; 0];
EqY = [0 ; -b(2)/A(2,2)];
Eq = [ Eq0 Eq1 EqX EqY ];


%%%
%
Resolution = 31;
[x1,x2] = meshgrid(linspace(0,LimX,Resolution),linspace(0,LimY,Resolution));

f1 = f1_fh(x1,x2);
f2 = f2_fh(x1,x2);
norm_f = sqrt(f1.^2 + f2.^2);
f1 = f1 ./ norm_f;
f2 = f2 ./ norm_f;


fig = figure(312);
delete(fig.Children);
Tl = tiledlayout(1,2,'TileSpacing','compact','Padding','compact');
ax = nexttile;
hold on, grid on, box on;

quiver(x1,x2,f1,f2,'DisplayName','Vector field')

for i = 1:size(Eq,2)
    plot(Eq(1,i),Eq(2,i),'.','MarkerSize',50)
end

term_event = @(t,x) hp_ode_terminal_event_ball(t,x,[0;0],norm([LimX,LimY]));
odeopts = odeset('Events',term_event);

initial_conditions = [
    0.1 0.1
    0.5 0.5
    0.8 0.8
    (0.1:0.2:3)' * [0 1] + [LimX 0]
    ];

xlim([0,LimX]);
ylim([0,LimY]);

N = size(initial_conditions,1);
It = 0;
for x0 = initial_conditions'
    title(sprintf('Press F5 %d times to continue [%d/%d]',N,It,N)), It = It + 1;
    keyboard
    %{
        Try also with ode23, ode45, ode23s, ode78, ode89
    %}
    [~,x] = ode89(f_fh,[0 1000],x0,odeopts);
    Pl = plot(x(1,1),x(1,2),'.','MarkerSize',25);
    plot(x(:,1),x(:,2),'Color',Pl.Color,'LineWidth',1.5);
end

title('Do not press F5 any more!')

ax = nexttile;
hold on, grid on, box on;

Tr = linspace(-3,3,101);
Pl = plot(Tr,Tr.^2/4);
for i = 1:size(Eq,2)
    J = J_fh(Eq(:,i));
    d = det(J);
    t = trace(J);

    plot(t,d,'.','MarkerSize',50)
end
XLim = ax.XLim;
xgrid = linspace(XLim(1),XLim(2),10000);
YLim = ax.YLim;
plot(XLim,[0 0],'Color',Pl.Color);
plot([0 0],YLim,'Color',Pl.Color);
plot(xgrid,xgrid.^2/4,'Color',Pl.Color);
ax.XLim = XLim;
ax.YLim = YLim;

