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

%{

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

%}

bA = {};
bA{1} = [
     1.4 , -0.5  -0.85 -1.5
    -1.4 ,  0.85  0.5   2.4
    -1.8 ,  3.4   2.6   0
    ];
 
bA{2} = [
     1.4 , -0.5  -0.85 -1.5
    -1.4 ,  0.85  0.5   2.4
     1.8 ,  3.4  -2.6  -1
    ];

bA = bA{1};

b = bA(:,1);
A = bA(:,2:end);

x = sym('x',[3,1]);
f = diag(x)*(A*x+b);
f_fh = @(t,x) diag(x)*(A*x+b);
f1_fh = matlabFunction(f(1),'vars',x);
f2_fh = matlabFunction(f(2),'vars',x);
f3_fh = matlabFunction(f(3),'vars',x);

Lims = [5,3,3];

xyz = 'xyz';
Coords = [
    1 2
    1 3 
    2 3
    ]';

term_event_2D = @(t,x) hp_ode_terminal_event_ball(t,x,[0;0],norm(Lims));
term_event_3D = @(t,x) hp_ode_terminal_event_ball(t,x,[0;0;0],norm(Lims));

initial_conditions = {
% (x,y)
  [ 0.1 0.1
    0.5 0.5
    0.8 0.8 ];
% (x,z)
  [ 0.1 0.1 ];
% (y,z)
  [ 0.030 3 
    0.032 3 
    0.033 3 
    0.035 3 
    0.040 3 
    2 0.002 
    2 0.006 
    2 0.010 
    2 0.014 
    2 0.018 ];
    };


fig = figure(312);
Tl = tiledlayout(1,1,'TileSpacing','compact','Padding','compact');
Ax1 = nexttile;
hold on, grid on, box on;


for dim = 1:size(Coords,2)
    coords = Coords(:,dim);
    A_ = A(coords,coords);
    b_ = b(coords);    
    Lims_ = Lims(coords);

    %%% 
    % Generate anonymous functions
    % 
    x = sym('x',[2,1]);
    f = diag(x)*(A_*x+b_);
    f_2D_fh = @(t,x) diag(x)*(A_*x+b_);
    J_fh = matlabFunction(jacobian(f,x),'vars',{x});
    f_c1_fh = matlabFunction(f(1),'vars',x);
    f_c2_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 ];
    Eq(:,any(Eq < 0,1)) = [];
        
    %%%
    %
    Resolution = 31;
    [x1,x2] = meshgrid(linspace(0,Lims(coords(1)),Resolution),linspace(0,Lims(coords(2)),Resolution));
    
    f1 = f_c1_fh(x1,x2);
    f2 = f_c2_fh(x1,x2);
    norm_f = sqrt(f1.^2 + f2.^2);
    f1 = f1 ./ norm_f;
    f2 = f2 ./ norm_f;

    O = x2*0;
    o = Eq(1,:)*0;
    switch xyz(coords)
        case 'xy'
            quiver3(x1,x2,O,f1,f2,O)
            plot3(Eq(1,:),Eq(2,:),o,'.','MarkerSize',50)
        case 'xz'
            quiver3(x1,O,x2,f1,O,f2)
            plot3(Eq(1,:),o,Eq(2,:),'.','MarkerSize',50)
        case 'yz'
            quiver3(O,x1,x2,O,f1,f2)
            plot3(o,Eq(1,:),Eq(2,:),'.','MarkerSize',50)
    end
    
    xlim([0,Lims(1)]);
    ylim([0,Lims(2)]);
    zlim([0,Lims(3)]);
    
    keyboard

    for x0 = initial_conditions{dim}'
        [~,x] = ode89(f_2D_fh,[0 1000],x0,odeset('Events',term_event_2D));

        O = x(:,1)*0;
        switch xyz(coords)
            case 'xy'
                Pl = plot3(x(1,1),x(1,2),0,'.','MarkerSize',25);
                plot3(x(:,1),x(:,2),O,'Color',Pl.Color,'LineWidth',1.5);
            case 'xz'
                Pl = plot3(x(1,1),0,x(1,2),'.','MarkerSize',25);
                plot3(x(:,1),O,x(:,2),'Color',Pl.Color,'LineWidth',1.5);
            case 'yz'
                Pl = plot3(0,x(1,1),x(1,2),'.','MarkerSize',25);
                plot3(O,x(:,1),x(:,2),'Color',Pl.Color,'LineWidth',1.5);
        end
    end

    keyboard

end

keyboard

Resolution = 11;
[X,Y,Z] = ndgrid(linspace(0,Lims(1),Resolution), ...
                 linspace(0,Lims(2),Resolution), ...
                 linspace(0,Lims(3),Resolution));

f1 = f1_fh(X,Y,Z);
f2 = f2_fh(X,Y,Z);
f3 = f3_fh(X,Y,Z);
norm_f = sqrt(f1.^2 + f2.^2 + f3.^2);
f1 = f1 ./ norm_f;
f2 = f2 ./ norm_f;
f3 = f3 ./ norm_f;
% quiver3(X,Y,Z,f1,f2,f3)

for x0 = [
        0.1 0.1 0.1
        0.1 0.1 0.00001
        0.5 2 0.000001
        [initial_conditions{1} initial_conditions{1}(:,1)*0+0.001]
        [initial_conditions{2}(:,1) initial_conditions{2}(:,1)*0+0.001 initial_conditions{2}(:,2)]
        ]'

    [~,x] = ode89(f_fh,[0 1000],x0,odeset('Events',term_event_3D));
    Pl = plot3(x(1,1),x(1,2),x(1,3),'.','MarkerSize',25);
    plot3(x(:,1),x(:,2),x(:,3),'Color',Pl.Color,'LineWidth',3);
end

xlabel 'x'
ylabel 'y'
zlabel 'z'

% (x,z)
view([0,0])

% (y,z)
view([90,0])

% 3D view
view([136 26])
