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

helper = Pcz_CasADi_Helper;

A = helper.new_var('A',[2,2]);
b = helper.new_var('b',[2,1]);

% helper.add_ineq_con('EqX',A(1,1)+b(1),'ub',0)
% helper.add_ineq_con('EqY',A(2,2)+b(2),'ub',0)

helper.add_ineq_con('Constraints_on_A',A(1,:)','lb',-10,'ub', 0)
helper.add_ineq_con('Constraints_on_A',A(2,:)','lb',  0,'ub',10)

helper.add_ineq_con('Constraints_on_b',b(1),'lb',0.1,'ub',  10)
helper.add_ineq_con('Constraints_on_b',b(2),'lb',-10,'ub',-0.1)

helper.add_ineq_con('Det_A_is_positive',det(A),'lb',0)

helper.add_ineq_con('Further_CONS',b(1)*A(2,2)-b(2)*A(1,2),'ub',-0.1)
helper.add_ineq_con('Further_CONS',b(2)*A(1,1)-b(1)*A(2,1),'ub',-0.1)

num_Trace = b(1)*A(2,2)*(A(2,1) - A(1,1)) + b(2)*A(1,1)*(A(1,2) - A(2,2));
helper.add_eq_con(num_Trace)
% helper.add_ineq_con('Trace_is_negative',num_Trace,'ub',-0.01)
% helper.add_ineq_con('Trace_is_positive',num_Trace,'lb',0.01)

% helper.add_obj('Simple_objective',sumsqr(b-1))

solver = helper.get_nl_solver;

sol = solver.solve;
A = helper.get_value(A);
b = helper.get_value(b);

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

%%

x = sym('x',[2,1]);
f = diag(x)*(A*x+b);
J = matlabFunction(jacobian(f,x),'vars',{x});

EqX = [-b(1)/A(1,1) ; 0]
[S,~] = eig(J(EqX))
lambda = eig(J(EqX))'

EqY = [0 ; -b(2)/A(2,2)]
[S,~] = eig(J(EqY))
lambda = eig(J(EqY))'

Eq = -A\b;
lambda = eig(J(Eq))

%%

LimX = 5;
LimY = 5;

%%% 
% 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],max(LimX,LimY));
odeopts = odeset('Events',term_event);
[t,x] = ode89(f_fh,[0 1000],[0.1;0.1],odeopts);
plot(x(1,1),x(1,2),'r.');
plot(x(:,1),x(:,2),'r');

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

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;

