%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%         Practice 2          %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Nonlinear Dynamical Systems %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%         2019.09.25.         %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%clear all;
%close all;

% damped pendulum equation: ddx+b*dx+sin(x)=0

%Exercises: analyse the system:
% - Show the equilibrium points!
%   - What kinds of fix points they are?
%       - Try to visualise the stable (stable spiral, sink) and the instable (saddle point) fix points!
%       - Which fix points are easy to show?
%           - Determine the separation line (separatrix) between two stable fix points!
%   - What is difference in cases of different damping parameter (see the shapes of the trajectories)?
% - Bolzano shooting: approach a saddle point with 1.0e-6 precison! (Give the initial x,y coordinates of the trajectory!) 

% parameters:
Xinit= -5;                 % initial value of x        %
Yinit= 2;                  % initial value of y        %
tinit=0;                   % initial time              % 
Max_Time= 150;             % end of the simulation     % 
b=0.5;%2.5;%               % damping                   % Change the damping value! What happens?

% 1. energy level curves:
x =-6*pi:0.05:6*pi;        % resolution of the grid
y =-6:0.05:6;              % resolution of the grid
[X,Y] = meshgrid(x,y);     % makes grid based on x and y vectors (for calculating the energies in each point)
energy_pendulum= @(X,Y) 0.5*Y.^2-cos(X)+1; % energy function: E=-cox(x)+1+(y^2)/2
Z =energy_pendulum(X,Y); % Energy level curves of the pendulum
% plot the energy levels with the contour function

% 2. calculate the gradient of the vector field in each grid point:
x2 =-6*pi:pi/5:6*pi;        % resolution of the grid
y2 =-6:0.5:6;              % resolution of the grid
[X2,Y2] = meshgrid(x2,y2);     % makes grid based on x and y vectors (for calculating the energies in each point)
% vector field:
dx=Y2; % change of x
dy=-b*Y2-sin(X2); % change of y
% direction field (vector field after normalisation):
%l=sqrt(dx.^2+dy.^2)
%dx=dx./l; %normalisation
%dy=dy./l;
%plot the vector field (quiver function)
% Which is the more informative/useful? (vector field or direction field)

% equation:
equ_8=@(t, xy) [xy(2); -b*xy(2)-sin(xy(1))]; %damped pendulum equation: ddx+b*dx+sin(x)=0

% visualisation
figure;
hold on;
%{
% 3. plot the trajectories
% 3.a. start from the X2, Y2 grid points: easy but slow:
sx=reshape(X2, 1, length(x2)*length(y2)); %the originak x2 points organised into a row vector (easyer to iterate throw)
sy=reshape(Y2, 1, length(x2)*length(y2)); %the originak y2 points organised into a row vector (easyer to iterate throw)
%}
%{
% 3.b. start trajectories form a line:
N=100; %number of points in the line
sx_start=0; % begin and end points of the line
sy_start=1;
sx_end=pi;
sy_end=2;
sx=linspace(sx_start,sx_end,N);
sy=linspace(sy_start,sy_end,N);
%}
%
%3.c trajectories starts from 2 lines:
N=100;
sx_start=0;
sy_start=1;
sx_end=pi;
sy_end=2;
sx2k=pi;
sy2k=-2;
sx2v=2*pi;
sy2v=-1;
% concatenate 2 point set
sx=[linspace(sx_start,sx_end,N), linspace(sx2k,sx2v,N)];
sy=[linspace(sy_start,sy_end,N), linspace(sy2k,sy2v,N)];
%}
traject_tol=0.01; %required accuracy
%calculate the differential equation for every point
for i=1:length(sx) 
    Xinit=sx(i);
    Yinit=sy(i); % calculate the equation from the given x,y point (the "hold on" is on, so plot the trajectories in the same figure)
    [t, xy]=ode45(equ_8, [tinit Max_Time], [Xinit; Yinit]); %calculate differential equation
    %
    [t2, xy2] = ode45(@(t, xy) -[xy(2); -b*xy(2)-sin(xy(1))], [tinit Max_Time], [Xinit; Yinit]); %reverse time: f(x)=-f(x)
    xy=[flipud(xy2); xy]; % the 2 outputs (normal and reverse time) are concatenated to one continous trajectory (the reverse time is turned at first)
    %}
    %plot(xy(:,1),xy(:,2), 'r');
    %
    % plot the separatrix: if a trajectory goes to one fix point, plot it with one color, if it goes to the other, plot it with the other color 
    if abs(xy(end,1)-0)<traject_tol % the end point of the trajectory is close to (in the x axis) which fix point
        plot(xy(:,1),xy(:,2), 'r');
    elseif abs(xy(end,1)-2*pi)<traject_tol
        plot(xy(:,1),xy(:,2), 'g');
    end
    %}
end
%}
%
[t, xy]=ode45(equ_8, [tinit Max_Time], [Xinit; Yinit]); %calculate differential equation
plot(xy(:,1), xy(:,2), 'r');
%}
quiver(X2,Y2,dx, dy, 'k'); % calculate the change of x and y in every X,Y grid ponts, and draw a vector from it starting from the X,Y point
energy_levels=[(linspace(sqrt(0),sqrt(2),5)).^2, (linspace(sqrt(2),sqrt(8),5)).^2, (linspace(sqrt(8),sqrt(16),5)).^2];% energy levels to calculate and plot
contour(X,Y,Z, energy_levels); % plot chosen energy levels
xlabel('x');
xticks(-6*pi:pi:6*pi)
ylabel('y');
xlim([-6*pi, 6*pi]);
ylim([-6, 6]);
hold off;