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

%{

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

%}

A = [
    -0.71796813279799299 -1.33982064934479705
     1.34705431078689442  0.67303593384500093
    ];

b = [
     0.55352737466824464
    -0.58064784752420218
    ];

LimX = 1;
LimY = 1;

%%% 
% 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);

Color_1 = [0 0.4470 0.7410];
Color_2 = [0.8500 0.3250 0.0980];
Color_3 = [0.9290 0.6940 0.1250];
Color_4 = [0.4940 0.1840 0.5560];
Color_5 = [0.4660 0.6740 0.1880];
Color_6 = [0.3010 0.7450 0.9330];
Color_7 = [0.6350 0.0780 0.1840];

initial_conditions = [
    0.01 0.01
    0.1 0.1
    1.0 0.05
    linspace(0.1,0.11,100)' * [0 1] + [LimX 0]
    linspace(0,2*LimY/3,10)' * [0 1] + [LimX 0]
    1.0 0.1
    1.0 0.10125
    1.0 0.1015
    1.0 0.10175
    1.0 0.10178
    1.0 0.10180
    1.0 0.10185
    1.0 0.1020
    1.0 0.1025
    1.0 0.105
    1.0 0.11
    1.0 0.15
    ];

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

N = size(initial_conditions,1);
It = 0;
for x0 = initial_conditions'
    [~,x] = ode45(f_fh,[0 1000],x0,odeopts);

    if norm(x(end,:) - Eq1) > LimX/10
        Color = Color_1;
    else
        Color = Color_2;
    end

    Pl = plot(x(1,1),x(1,2),'.','Color',Color,'MarkerSize',25);
    plot(x(:,1),x(:,2),'Color',Pl.Color,'LineWidth',1.5);
end

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;

