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

syms t x1 x2 real
x = [x1;x2];

A = sym('a%d%d',[2,2]);
b = sym('b%d',[2,1]);

f = x .* (A*x + b);

J = jacobian(f,x);

% Normalized vector field
Norm_f = norm(f);

f1 = f(1) / Norm_f;
f2 = f(2) / Norm_f;

f_fh = matlabFunction(f,'Vars',{x,A,b});
f1_fh = matlabFunction(f1,'Vars',{x1,x2,A,b});
f2_fh = matlabFunction(f2,'Vars',{x1,x2,A,b});

J_fh = matlabFunction(J,'Vars',{x,A,b});

clear A b J f1 f2

%%

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];

Colors = [
    Color_1
    Color_2
    Color_3
    Color_4
    ];

while true
    
    b3 = round(randn(3,1),2);
    A3 = round(randn(3,3),2);    

    fig = figure(312);
    delete(fig.Children);
    Tl = tiledlayout(3,2,'TileSpacing','compact','Padding','compact','TileIndexing','rowmajor');
    

    GOOD = 1;
    for c = [
        1 1 2
        2 3 3 ]
    
        A = A3(c,c);
        b = b3(c);
    
        Eq0 = [0;0];
        Eq1 = -A\b;
        
        A1 = J_fh(Eq1,A,b);
    
        LaTeX = sprintf([ '$'...
            '\\left\\{\\matrix{ ' ...
            '    \\dot x = x (%g %+g x %+g y) ' ...
            '    \\cr' ...
            '    \\dot y = y (%g %+g x %+g y) ' ...
            '}\\right.' ...
            '$'],b(1),A(1,:),b(2),A(2,:));
        
        EqX = [-A(1,1)\b(1) ; 0];
        EqY = [0 ; -A(2,2)\b(2)];
        
        Eq = [ Eq1 EqX EqY ];
        
        % Remove equilibrium points, which do not belong the the positive quadrant
        Eq(:,any(Eq < 0,1)) = [];
    
        n = vecnorm(Eq,1);
        Eq(:,n < eps | isnan(n) | isinf(n)) = [];
    
        Eq = [ Eq0 Eq ];
        
        J = J_fh(Eq1,A,b);
        d = det(J);
        t = trace(J);
            
        if size(Eq,2) < 4
            GOOD = 0;
            break
        end    
        if d < t^2/4 
            GOOD = 0;
            break
        end

        Jx = J_fh(EqX,A,b);
        [Sx,Dx] = eig(Jx);
        Sx(:,2) = Sx(:,2) * sign(prod(Sx(:,2) + EqX)) * sign(Dx(2,2)); % Elojel valtas, ha szukseges
    
        Jy = J_fh(EqY,A,b);
        [Sy,Dy] = eig(Jy);
        Sy(:,2) = Sy(:,2) * sign(prod(Sy(:,2) + EqY)) * sign(Dy(2,2)); % Elojel valtas, ha szukseges
    
        Lim = 1.2*max(Eq(:));
        Lim = 5;
        lspace = linspace(0,Lim,31);
        [x1,x2] = meshgrid(lspace);
        
        f1 = f1_fh(x1,x2,A,b);
        f2 = f2_fh(x1,x2,A,b);
        
        ax = nexttile;
        hold on, grid on, box on;
        
        quiver(x1,x2,f1,f2,'DisplayName','Vector field')
    
        if EqX(1) > 0
            quiver(EqX(1),EqX(2),Sx(1,2),Sx(2,2),1,'LineWidth',2,'ShowArrowHead','off','Color',Color_2)
        end
        if EqX(2) > 0
            quiver(EqY(1),EqY(2),Sy(1,2),Sy(2,2),1,'LineWidth',2,'ShowArrowHead','off','Color',Color_2)
        end
    
        for i = 1:size(Eq,2)
            plot(Eq(1,i),Eq(2,i),'.','MarkerSize',50,'Color',Colors(i,:))
        end
        
        axis equal
        title(LaTeX,'Interpreter','latex','FontSize',14)
        
        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),A,b);
            d = det(J);
            t = trace(J);
        
            plot(t,d,'.','MarkerSize',50,'Color',Colors(i,:))
        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;
    
        pcz_num2str_multiline(A,b)

    end

    if ~GOOD
        continue
    end

    fig = figure(312);
    delete(fig.Children);
    ax = axes(fig); hold on;
    
    f_ode = @(t,x) diag(x)*(A3*x+b3);
    term_event = @(t,x) hp_ode_terminal_event_ball(t,x,[0;0;0],10);
    args = odeset('Events',term_event);
    for x0 = abs(randn(3,10))+eps
        [~,x] = ode45(f_ode,[0 1000],x0,args);
        Pl = plot3(x(1,1),x(1,2),x(1,3),'.','MarkerSize',25);
        plot3(x(:,1),x(:,2),x(:,3),'Color',Pl.Color)
    end
    

    break
    % Leg = legend('Interpreter','latex','FontSize',14, ...
    %     'Location','eastoutside','Box','off');

end

%%

A = [
      -0.300    0.300
      -1.100    0.300
    ];

b = [
      -0.200
       0.800
    ];

[a11,a21,a12,a22] = deal(A(1),A(2),A(3),A(4));
[b1,b2] = deal(b(1),b(2));

TEST = (a22/a11) * (b1/b2) * ( (a21-a11)/(a22-a12) )

a12 = -(a22/a11) * (b1/b2) * ( (a21-a11) ) + a22
A(1,2) = a12;

%%

% Volt:
A = [
      -0.100   -0.300
       1.300    0.400
    ];

b = [
       0.500
      -1.400
    ];    

% Lett:
A = [
      -0.200   -0.600
       1.300    0.300
    ];

b = [
       0.600
      -1.500
    ];    

[a11,a21,a12,a22] = deal(A(1),A(2),A(3),A(4));
[b1,b2] = deal(b(1),b(2));

TEST = (a22/a11) * (b1/b2) * ( (a21-a11)/(a22-a12) )

a12 = -(a22/a11) * (b1/b2) * ( (a21-a11) ) + a22
A(1,2) = a12;

%%

% Volt:
A = [
      -0.110    1.060
      -0.790   -0.240
    ];

b = [
      -0.210
       0.600
    ];

[a11,a21,a12,a22] = deal(A(1),A(2),A(3),A(4));
[b1,b2] = deal(b(1),b(2));

TEST = (a22/a11) * (b1/b2) * ( (a21-a11)/(a22-a12) )

Eq1 = -A\b

Trace = trace(J_fh(Eq1,A,b))

% -(a22/a11) * (b1/b2) * ( (a21-a11) ) + a22
% A(1,2) = a12;

