%% Searching for a piecewise quadratic Lyapunov function for a piecewise linear system
% Example 4.3, page 47, from Johansson, Mikael K.-J. Piecewise Linear Control Systems: A Computational Approach. Lecture Notes in Control and Information Sciences. Berlin, Heidelberg: Springer, 2003. https://doi.org/10.1007/3-540-36801-9.
% A dynamical system is considered that is described by the differential equation
% \dot x = A_ix,
% where i is an index of the cell in the state plane, the matrices are given by

epsilon = 0.1;
omega = 1;
alpha = 5;

A1 = [-epsilon, omega; -alpha*omega, -epsilon];
A3 = A1;

A2 = [-epsilon, alpha*omega; -omega -epsilon];
A4 = A2;

%
%% Phase portrait

[X1,X2] = meshgrid(-1.2:0.1:1.2,-1.2:0.1:1.2);
F1a = A1(1,1)*X1 + A1(1,2)*X2; 
F2a = A1(2,1)*X1 + A1(2,2)*X2;

F1b = A2(1,1)*X1 + A2(1,2)*X2; 
F2b = A2(2,1)*X1 + A2(2,2)*X2;

F1 = F1a;
F2 = F2a;
ix = abs(X2) <= abs(X1); % Can check the "mask" using the spy function.
F1(ix) = F1b(ix);
F2(ix) = F2b(ix);

figure(1)
quiver(X1,X2,F1,F2,2)
xlabel('x_1')
ylabel('x_2')
axis equal
%streamslice(X,Y,DX,DY)
hold on
fimplicit(@(x1,x2) x2-x1,'LineWidth',2)
fimplicit(@(x1,x2) x2+x1,'LineWidth',2)
axis([-1.2,1.2,-1.2,1.2])
hold off

%
%% Solving the hybrid equations

sbsf = SwitchBetweenStableFlower();

% Initial conditions
x0 = [-1;0.5];
q0 = 1;

% Time spans
tspan = [0, 100];
jspan = [0, 30];

% Specify solver options.
config = HybridSolverConfig('AbsTol', 1e-7, 'RelTol', 1e-7);

% Compute solution
sol = sbsf.solve([x0;q0], tspan, jspan, config);

%
%% Plotting the solution of a switched system

figure(2)
%plotFlows(sol)
%plotHybrid(sol)
hp = HybridPlotBuilder().plotPhase(sol.select([1,2]))
hold on
fimplicit(@(x1,x2) x2-x1)
fimplicit(@(x1,x2) x2+x1)
axis([-1.2,1.2,-1.2,1.2])
%axis equal
grid on
hold off

%
%% Multiple Lyapunov function
%% Describing the partition of the state space using an LMI

c = [1; 1];
d = [-1; 1];

Q1 = (c*d' + d*c')/2
Q2 = -Q1
I = eye(2,2);

%
%% Setting up an LMI for MLF

P1 = sdpvar(2,2);
P2 = sdpvar(2,2);

u1 = sdpvar(1,1);
u2 = sdpvar(1,1);

z1 = sdpvar(1,1);
z2 = sdpvar(1,1);

F = [u1 >= 0, u2 >= 0, z1 >= 0, z2 >= 0, ...
    P1-u1*Q1 >= I, ...
    P2-u2*Q2 >= I, ...
    A1'*P1+P1*A1+z1*Q1 <= -I, ...
    A2'*P2+P2*A2+z2*Q2 <= -I, ...
    c'*(P1-P2)*c == 0, ...
    d'*(P2-P1)*d == 0
    ];

options = sdpsettings('verbose',1,'solver','sedumi');

lmisol = optimize(F,[],options);
lmisol.info

%

P1 = value(P1)
P2 = value(P2)

%
%% Plotting the solution obtained by solving an LMI

figure(3)
hp5 = HybridPlotBuilder().plotFlows(sol,@(x) mlf(x,P1,P2));
xlabel('t')
ylabel('V(x(t))')
title('Piecewise quadratic Lyapunov function')
grid on

%
%% Plotting the piecewise quadratic Lyapunov function

figure(4)
x1 = -1.0:.01:1.0;
x2 = -0.5:.01:0.5;
[X1,X2] = meshgrid(x1,x2);
V = zeros(size(X1));
for i = 1:length(x2)
    for j = 1:length(x1)
        x = [x1(j);x2(i)];
        V(i,j) = mlf(x,P1,P2);
    end
end

mesh(X1,X2,V)
xlabel('x_1')
ylabel('x_2')
zlabel('V(x)')
title('Piecewise quadratic Lyapunov function')

%
% Matlab seems not to publish the previous figure if no commmand follows. Let's give it to him.

finished = "yes";

%
%% Definition of one particular Multiple Lyapunov Function (MLF)
% In this file we only use it for the purpose of plotting the evolution of its value. Here we just accept it as given, we do not show how to find it. This we will show elsewhere.

function V = mlf(x,P1,P2)
    if abs(x(2))>=abs(x(1)) 
        V = x(1:2)'*P1*x(1:2);
    else
        V = x(1:2)'*P2*x(1:2);
    end
end