%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%         Practice 4          %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Nonlinear Dynamical Systems %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%         2019.10.09.         %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Parameters:
h=0.1;                      % stepsize
N=500;                      % step number

x_null=0;                   % initial conditions
y_null=1;
t_null=0;

% Calculate time
% All methods use the same time points
time=t_null:h:(N-1)*h;

% TASKS
% The general perfect string equation: x"+x = 0
% Rewrite to 2 1st order equation:
% x'= y
% y'=-x 

% Analytic solution:
% dx(t) = cos(t)
% dy(t) = -sin(t)


% ode45:
rugo_ode=@(t,xy) [xy(2); -xy(1)];
[timeode45, ode45mg]=ode45(rugo_ode, [t_null, h*N], [x_null, y_null]);

% Energy function (Eucledian norm):
NRG_ode45=(ode45mg(:,1).*ode45mg(:,1)+ode45mg(:,2).*ode45mg(:,2))/2;


% Analytic solution:

analytic=zeros(N,2);
analytic(1,1)=x_null;
analytic(1,2)=y_null;

for a=1:(N-1)
    analytic(a+1,1) = sin(time(a+1));
    analytic(a+1,2) = cos(time(a+1));
end

% Energy function (Eucledian norm):
NRG_AN=(analytic(:,1).*analytic(:,1)+analytic(:,2).*analytic(:,2))/2;



% Explicit Euler method
% (X-x)/h = y
% (Y-y)/h =-x

EEuler=zeros(N,2);

EEuler(1,1)=x_null;
EEuler(1,2)=y_null;

for a=1:(N-1)
    EEuler(a+1,1) = EEuler(a,1) + h .* EEuler(a,2);
    EEuler(a+1,2) = EEuler(a,2) + h .* (-EEuler(a,1));
end

% Energy function (Eucledian norm):
NRG_EE=(EEuler(:,1).*EEuler(:,1)+EEuler(:,2).*EEuler(:,2))/2;



%Implicit Euler method
% (X-x)/h = Y
% (Y-y)/h =-X
IEuler = zeros(N,2);

IEuler(1,1)=x_null;
IEuler(1,2)=y_null;

for a = 1 : (N-1)
    IEuler(a+1,1)  = (IEuler(a,1)  + (h *IEuler(a,2))) / (1+h*h);
    IEuler(a+1,2)  = (IEuler(a,2)- (h * IEuler(a,1))) / (1+h*h);
end

% Energy function (Eucledian norm):
NRG_IE=(IEuler(:,1).*IEuler(:,1)+IEuler(:,2).*IEuler(:,2))/2;



% Semi-implicit Euler method:
   % (X-x)/h =y
   % (Y-y)/h =-X
SEuler = zeros(N,2);

SEuler(1,1)=x_null;
SEuler(1,2)=y_null;

for a = 1 : (N-1)
    SEuler(a+1,1)  = SEuler(a,1) + h * SEuler(a,2);
    SEuler(a+1,2)  = SEuler(a,2) - h * (SEuler(a,1) + h * SEuler(a,2)); %  SEuler(a+1,2) = SEuler(a,2) - h * SEuler(a+1,1);
end

% Energy function (Eucledian norm):
NRG_SE=(SEuler(:,1).*SEuler(:,1)+SEuler(:,2).*SEuler(:,2))/2;




%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%        Plotting        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


figure;
%in the function of t
subplot(2,2,1);
plot(time,analytic(:,1),'black');
hold on;
plot(time,EEuler(:,1),'red');
plot(time,IEuler(:,1),'green');
plot(time,SEuler(:,1),'blue');
plot(timeode45, ode45mg(:,1),'magenta');
hold off;
% all plot in one:
%plot(time,analytic(:,1),'black',time,EEuler(:,1),'red',time,IEuler(:,1),'green',time,SEuler(:,1),'blue', timeode45, ode45mg(:,1),'magenta');
xlabel('t');
ylabel('x');
axis equal
legend('Anal', 'EE', 'IE', 'SE', 'ode45');

%x-y 
subplot(2,2,2)

plot(analytic(:,1),analytic(:,2),'black');
hold on;
plot(EEuler(:,1),EEuler(:,2),'red');
plot(IEuler(:,1),IEuler(:,2),'green');
plot(SEuler(:,1),SEuler(:,2),'blue');
plot(ode45mg(:,1), ode45mg(:,2), 'magenta');
hold off;
% all plot in one:
%plot(analytic(:,1),analytic(:,2),'black',EEuler(:,1),EEuler(:,2),'red',IEuler(:,1),IEuler(:,2),'green',SEuler(:,1),SEuler(:,2),'blue', ode45mg(:,1), ode45mg(:,2), 'magenta')
xlabel('x');
ylabel('y');
axis equal
legend('Anal', 'EE', 'IE', 'SE', 'ode45');

%Energies
subplot(2,2,3);

plot(time,NRG_AN,'black');
hold on;
plot(time,NRG_EE,'red');
plot(time,NRG_IE,'green');
plot(time,NRG_SE,'blue');
plot(timeode45, NRG_ode45,'magenta');
hold off;
% all plot in one:
%plot(time,NRG_AN,'black',time,NRG_EE,'red',time,NRG_IE,'green',time,NRG_SE,'blue', timeode45, NRG_ode45,'magenta')
xlabel('t');
ylabel('Energy');
axis equal
legend('Anal', 'EE', 'IE', 'SE', 'ode45');

%t-x-y
subplot(2,2,4);

plot3(time,analytic(:,1),analytic(:,2),'black');
hold on;
plot3(time,EEuler(:,1),EEuler(:,2),'red');
plot3(time,IEuler(:,1),IEuler(:,2),'green');
plot3(time,SEuler(:,1),SEuler(:,2),'blue');
plot3(timeode45, ode45mg(:,1), ode45mg(:,2),'magenta');
hold off;
% all plot in one:
%plot3(time,analytic(:,1),analytic(:,2),'black',time,EEuler(:,1),EEuler(:,2),'red',time,IEuler(:,1),IEuler(:,2),'green',time,SEuler(:,1),SEuler(:,2),'blue', timeode45, ode45mg(:,1), ode45mg(:,2),'magenta')
grid on
xlabel('t');
ylabel('x');
zlabel('y');
legend('Anal', 'EE', 'IE', 'SE', 'ode45');
