% Global stability analysis using sosopt
% Form the system
a = 1.1;
pvar x y
f = [1/a*(x - (1/3)*(x-a)^3 - y - a^3/3);x];
z = [x;y];
p = z'*z;
f = cleanpoly(f,1e-5);


% Define the monomials vector to form V
% Use all monomials in x and y of degree 2
zV = monomials(z,2:2);
V = polydecvar('c',zV,'vec');
% Constraint 1 : V(x) - L1 \in SOS
L1 = 1e-6 * ( x^2 + y^2 );
sosconstr{1} = V - L1;
% Constraint 2: -Vdot - L2 \in SOS
L2 = 1e-6 * ( x^2 + y^2 );
Vdot = jacobian(V,z)*f;
sosconstr{2} = -Vdot - L2;
% Solve with feasibility problem
[info,dopt,sossol] = sosopt(sosconstr,z);
% check the feasibility
% info.feas == 1 is feasible
% info.feas == 0 is infeasible
info.feas

% Try increasing the degree of V
% V potentially includes all monomials of degree 
% 2, 3, and 4.
zV = monomials(z,2:4);
V = polydecvar('c',zV,'vec');
sosconstr{1} = V - L1;
Vdot = jacobian(V,z)*f;
sosconstr{2} = -Vdot - L2;
[info,dopt,sossol] = sosopt(sosconstr,z);
info.feas

% Increase the degree of V to 6
zV = monomials(z,2:6);
V = polydecvar('c',zV,'vec');
sosconstr{1} = V - L1;
Vdot = jacobian(V,z)*f;
sosconstr{2} = -Vdot - L2;
[info,dopt,sossol] = sosopt(sosconstr,z);
info.feas
V = subs(V,dopt);


% Plot some of the sublevel sets of V
[C,h] = pcontour(V, [1,2,3,4],[-3 3 -2 4]);
grid on;
hold on;
set(h,'linewidth',2);

% Plot some trajectories
% set the initial conditions
X = [1 -1; 2 -1;2 0; 2 1; 2 2; 2 3; 2 4; 1 4;
    0 4; -1 4; -2 4; -2 3; -2 2; -2 1;-2 0];
[xtraj,xconv] = psim(f,z,X',10);
for i1 = 1:size(X,1)
    plot(xtraj{i1,2}(:,1),xtraj{i1,2}(:,2),'r');
    plot(xtraj{i1,2}(1,1),xtraj{i1,2}(1,2),'k*');
end