%% Verification of stability of a switched system using Multiple Lyapunov Function
% Example 4.11 from Lin & Antsaklis (2022). Hybrid automation with two discrete states, in each state the continuous dynamics is characterized using a second-order state equation

n = 2;

A1 = [-0.1, -1; 2, -0.1];
A2 = [-0.1, -2; 1, -0.1];

%
% The dynamics of each discrete state is obviously stable

eig(A1)
eig(A2)

%
%% C(Q)LF computing
% Let's now search for a Common Quadratic Lyapunov function

P = sdpvar(n,n);
I = eye(n,n);

F = [P >= I, A1'*P+P*A1 <= -I; A2'*P+P*A2 <= -I];

optimize(F);
value(P)

%
% Obviously we did not succeed in finding one. Does it mean that the system is unstable? Not really, see the next section.
%% Simulation of the response of the switched system
% We build a model of a switched system using HybridEquations toolbox

smlf = SwitchBetweenStableMLF();

% We set the initial conditions to
x0 = [1; 1];
q0 = 1;

% We also set the time spans for both the continuous time and number of
% discrete transitions (jumps)

tspan = [0, 20];
jspan = [0, 30];

% We can also specify some solver options
config = HybridSolverConfig('AbsTol', 1e-8, 'RelTol', 1e-8);

% Finally we compute the solution
sol = smlf.solve([x0;q0], tspan, jspan, config);

%
%% Plotting the solutions of a switched systems
% First, we can plot trajectories for the two continuous state variables and the (single) discrete state variable.

plotFlows(sol)

%
% But what is certainly a bit more illuminating is the plot of state trajectories in the (continuous) state plane.

clf
hp1 = HybridPlotBuilder().plotPhase(sol.select([1,2]));
grid on
hold on
P1 = diag([2,1]);
P2 = diag([0.5,1]);
fimplicit(@(x1,x2) [x1;x2]'*P1*[x1;x2]-1,'LineStyle','--','Color','r')
fimplicit(@(x1,x2) [x1;x2]'*P2*[x1;x2]-1,'LineStyle','--','Color','k')
hold off

%
% Besides the state trajectories we also plot two ellipses corresponding to sublevel sets (for some particular value) of the two Lyapunov functions.
%% Plotting evolution of one particular Lyapunov function
% Finally, let's now plot the evolution of some candidates for a Lyapunov function. We can start with the Lyapunov function for one of the systems

clf
hp3 = HybridPlotBuilder().plotFlows(sol,@(x) x(1:2)'*P2*x(1:2));
xlabel('t')
ylabel('V(x(t))')
title('Lyapunov-like function')
grid on

%
% Obviously this function does not qualify as Lyapunov function because it is not decreasing. Try some other candidates (Lyapunov function for the other system, or perhaps just the norm of x). But it is a lost game. At least when restricting ourselves to quadratic functions, because the LMI solver has failed to find such Lyapunov function.
% But we can try some Multiple Lyapunov Function (MLF) obtained by patching together Lyapunov-like functions defined over the respective partitions of the state space (the 2D plane in this case).

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

%
%% Describing the partition of the state space using an LMI

r1 = [1; 0];         % Defines a halfplane r'*x>=0. Here x1>=0.
Q1 = zeros(3,3);
Q1(1:2,3) = r1;
Q1(3,1:2) = r1';

Q2 = -Q1; 

I = eye(3,3);
I(3,3) = 0;

A1bar = zeros(3,3);
A1bar(1:2,1:2) = A1;
A1 = A1bar;

A2bar = zeros(3,3);
A2bar(1:2,1:2) = A2;
A2 = A2bar;

%
%% Setting up an LMI for MLF

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

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

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

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

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

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

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

%

P1bar = value(P1)
P2bar = value(P2)
P1 = P1bar(1:2,1:2)
P2 = P2bar(1:2,1:2)

%
%% Plotting the solution obtained by solving an LMI

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

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. Well, then

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 x(1)>=0
        V = x(1:2)'*P1*x(1:2);
    else
        V = x(1:2)'*P2*x(1:2);
    end
end