%% Lyapunov function for pendulum
%%
%% Parameters
%%
m = 1;
l = 1;
g = 10;
b = 0.2;

x0 = [-3*pi;2*pi];
eigvals = [-0.01 -0.0];
T = 20;
xlims = [-5*pi,5*pi];
ylims = [-3*pi,3*pi];
threshold = Inf;
%% 
% Declaration of symbolic variables

syms alpha t x1 x2 real
x = [x1;x2];
%% State space equation $\dot x = f(x)$
%%
f = [
    x2
    -g/l*sin(x1) - b/m*x2
    ]
%% Lyapunov function $V(x)$
% Consider the following Lyapunov function
%%
V = m*g*l*(1-cos(x1)) + 1/2*m*l^2*x2^2;
%% 
% We generate Matlab typed function handles

V_fh = matlabFunction(V, 'vars', x);
f_ode = matlabFunction(f, 'vars', {t x});
f1_fh = matlabFunction(f(1), 'vars', x);
f2_fh = matlabFunction(f(2), 'vars', x);
%% 
% Simulate the system $\dot x(t) = f(x(t)), ~ x(0) = x_0$, where $x_0$ is 
% the initial condition

[tt,xx] = ode45(f_ode, linspace(0,T,100*T), x0);
%% 
% Compute the value of the Lyapunov function along the trajectory: $V(t) 
% := V(x(t))$

V_xx = V_fh(xx(:,1), xx(:,2));
%% 
% Create a meshgrid in order to plot the Lyapunov function

res = 300;
[x_mesh,y_mesh] = meshgrid(...
    linspace(xlims(1),xlims(2),res),...
    linspace(ylims(1),ylims(2),res));
V_mesh = V_fh(x_mesh,y_mesh);
V_mesh(V_mesh > threshold) = NaN;
%% 
% Generate the vector field to visualize $f(x)$

res = 16;
[x_mesh2,y_mesh2] = meshgrid(...
    linspace(xlims(1),xlims(2),res),...
    linspace(ylims(1),ylims(2),res));
v1 = f1_fh(x_mesh2,y_mesh2);
v2 = f2_fh(x_mesh2,y_mesh2);
%% 
% Normalize the vector field in order that the directions of the trajectories 
% be more visible

r = sqrt(v1.^2 + v2.^2);
v1 = v1 ./ r;
v2 = v2 ./ r;
%% Plot
%%

energy_levels = m*g*l * (0:0.5:5);

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

fs = 20;
Ts = 1/fs;

xtlin = interp1(tt,xx,linspace(0,tt(end),T*fs));
V_xtlin = interp1(tt,V_xx,linspace(0,tt(end),T*fs));

phi = xtlin(:,1);

fig = figure(123);
fig.Position(3:4) = [ 1238 583 ];
Tl = tiledlayout(1,3);
pause(0.5)

Ax1 = nexttile([1,2]); hold on, grid on, box on
view([-23.2 42.3])
xlim(xlims)
ylim(ylims)
% axis vis3d
plot(xx(1,1), xx(1,2),'.k','markersize',15)
plot3(xx(1,1), xx(1,2),V_xx(1),'.k','markersize',15)
surf(x_mesh,y_mesh,V_mesh,'facealpha',0.5)
Pl2 = plot(xtlin(:,1), xtlin(:,2),'Color',Color_2,'LineWidth',2);
Pl3 = plot3(xtlin(:,1), xtlin(:,2),V_xtlin,'Color',Color_3,'LineWidth',2);
shading interp
contour(x_mesh,y_mesh,V_mesh,energy_levels,'LineWidth',1.2)
% quiver(x_mesh2,y_mesh2,v1,v2,0.5, 'Color', [1 1 1]*0.5);
daspect([2 2 6])
xlabel('$x_1 = y$ [rad]','Interpreter','latex')
ylabel('$x_2 = \dot{y}$ [rad/s]','Interpreter','latex')
Ax1.TickLabelInterpreter = "latex";
Ax1.FontSize = 16;

Ax2 = nexttile; hold on, grid on, box on
plot(tt,V_xx), grid on
title('$V(t) := V(x(t))$', 'interpreter', 'latex','fontsize',22)
xlabel('time $t$ [s]', 'interpreter', 'latex','fontsize',22)
Ax2.TickLabelInterpreter = "latex";
Ax2.FontSize = 16;

%% 

delete(Ax2)
Ax2 = nexttile; hold on, grid on, box on
IngaPl = plot([0,sin(phi(1))],[0,-cos(phi(1))],'LineWidth',10);
plot(0,0,'k.','MarkerSize',40)
axis([-2,2,-2,2]/2*1.2)
daspect([1 1 1])
title({'Illustrating the movement of the pendulum','$ml\ddot{y} + bl\dot{y} + mg\sin{y}=0$'},'Interpreter','latex')


tic;
for i = 2:numel(phi)
    Pl2.XData = xtlin(1:i,1);
    Pl2.YData = xtlin(1:i,2);
    Pl3.XData = xtlin(1:i,1);
    Pl3.YData = xtlin(1:i,2);
    Pl3.ZData = V_xtlin(1:i);

    IngaPl.XData = [0,sin(phi(i))];
    IngaPl.YData = [0,-cos(phi(i))];

    drawnow
    dt = toc;
    tic;
    frames(i) = getframe(fig);
    pause(Ts - dt);
end
