%close all;

%Inga egyenlet
%x'=y
%y'=-sin(x)

% Energiafuggveny: 0.5*y*y + 1-cos(x)

%Kezdeti ertekek specialisan:
%x(0)=pi/2
%y(0)=0

%4 Numerikus modszert vizsgalunk.
%Azt tapasztaljuk, hogy minosegileg maskepp viselkednek a parameterek
%fuggvenyeben.
%Kiszamoljuk a rendszerek energiait es abrazoljuk kulonfele koordinatak
%szerint az egyes megoldasokat.

%Jeloles magyarazat:

%StepSize === h
%NumOfSteps === N (ertsd: 1->N)
%time === [ t_0 t_1 ...t_n... t_N-1 ]
%EEuler === Explicit Euler modszer matrixa
%IEuler === Implicit Euler modszer matrixa
%SEuler === "Semi-Euler" modszer matrixa
%NRG_*E === A rendszer energiajanak vektora az ido szerint
%NRG_AN === A rendszer energiajanak vektora az ido szerint

%Matrix jeloles:
% Matrix/Vektor elso elemenek indexe a Matlabban 1!!!
%*Euler(a,1) === x === x_n
%*Euler(a,2) === y === y_n
%*Euler(a+1,1) === X === x_n+1
%*Euler(a+1,2) === Y === y_n+1

%FELADAT:
%1) Implementaljuk az egyes modszereket a jeloleseknek megfeleloen
%2) Probaljunk ki mas lepeskoz es lepesszam kombinaciokat! Mit latunk?
%3) Nezzuk meg az energia fuggvenyeket is!
StepSize=0.1;%0.1;%0.001;%
NumOfSteps=100;%1000;%100000;%

x_null=pi/2;
y_null=0;
t_null=0;

% Ido kiszamolasa.
%Minden modszer azonos diszkret ido-pontokat hasznal, ezert itt kulon van
%szedve.
time=zeros(NumOfSteps,1);
time(1)=t_null;

for a=1:(NumOfSteps-1)
    time(a+1)=time(a)+StepSize;
end

%Explicit euler method
EEuler=zeros(NumOfSteps,2);

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

% X = x +h*y
% Y = y -h*sin(x)
for a=1:(NumOfSteps-1)
    EEuler(a+1,1) = EEuler(a,1) + StepSize * EEuler(a,2);
    EEuler(a+1,2) = EEuler(a,2) - StepSize * sin(EEuler(a,1));
end

%Energiafuggveny
NRG_EE=EEuler(:,2).*EEuler(:,2)./2.+1.-cos(EEuler(:,1));

%Implicit Euler method
IEuler = zeros(NumOfSteps,2);

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

eps = 1.0e-6; % Fixpont tetel toleranciaja

% Ennek a fixpontjat keressuk:
%X_{i+1} = x + h*y - h*h*sin(X_i);

%X_f fixpontra megoldjuk az egyenletet
%X = X_f; % Ettol implicit
%Y = y - h*sin(X); % Ettol implicit

for a = 1 : (NumOfSteps-1)
    
    % Fixpont tetel
    X(1) = IEuler(a,1);
    i=1;
    done=false;
    
    while ~done %iterációval gyökkeresés
        X(i+1) = IEuler(a,1)+StepSize*IEuler(a,2) - StepSize*StepSize*sin(X(i));
        i=i+1;
        if abs(X(i)-X(i-1)) < eps
            done= true;
        end
    end
    IEuler(a+1,1) = X(i); % Ettol implicit
    IEuler(a+1,2) = IEuler(a,2) - StepSize*sin(IEuler(a+1,1)); % Ettol implicit
end

%Energiafuggveny
NRG_IE=IEuler(:,2).*IEuler(:,2)./2.+1.-cos(IEuler(:,1));

%"Semi-euler"
SEuler = zeros(NumOfSteps,2);

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


% X = x + h*y
% Y = y - h*sin(X)
for a = 1 : (NumOfSteps-1)
    SEuler(a+1,1)  = SEuler(a,1) + StepSize * SEuler(a,2);
    SEuler(a+1,2)  = SEuler(a,2) - StepSize * sin(SEuler(a,1) + StepSize*SEuler(a,2));
end

%Energiafuggveny
NRG_SE=SEuler(:,2).*SEuler(:,2)./2.+1.-cos(SEuler(:,1));


%ode45
rugo_ode=@(t,xy) [xy(2); -sin(xy(1))];
[timeode45, ode45mg]=ode45(rugo_ode, [t_null, StepSize*NumOfSteps], [x_null, y_null]);

%Euklideszi norma
NRG_ode45=0.5*ode45mg(:,2).^2.+1.-cos(ode45mg(:,1));


figure
%t-szerint
subplot(2,2,1);
plot(time,EEuler(:,1),'red',time,IEuler(:,1),'green',time,SEuler(:,1),'blue',timeode45,ode45mg(:,1),'magenta','DisplayName', 't szerint');
legend('EE','IE','SE','ode45');
axis equal
xlabel('time')
ylabel('x')

%x-y szerint
subplot(2,2,2);
plot(EEuler(:,1),EEuler(:,2),'red',IEuler(:,1),IEuler(:,2),'green',SEuler(:,1),SEuler(:,2),'blue',ode45mg(:,1),ode45mg(:,2),'magenta','DisplayName', 'x-y szerint')
legend('EE','IE','SE','ode45');
axis equal
xlabel('x')
ylabel('y')

%Energiak
subplot(2,2,3);
plot(time,NRG_EE,'red',time,NRG_IE,'green',time,NRG_SE,'blue',timeode45,NRG_ode45,'magenta','DisplayName', 'Energiak')
legend('EE','IE','SE','ode45');
axis equal
xlabel('time')
ylabel('energia')

%t-x-y szerint
subplot(2,2,4);
plot3(time,EEuler(:,1),EEuler(:,2),'red',time,IEuler(:,1),IEuler(:,2),'green',time,SEuler(:,1),SEuler(:,2),'blue',timeode45,ode45mg(:,1),ode45mg(:,2),'magenta','DisplayName', 't-x-y szerint')
legend('EE','IE','SE','ode45');
xlabel('time')
ylabel('x')
zlabel('y') 
grid on
