% Paraméterek:
h=0.1;                      % lepeskoz
N=500;                      % lepesszam

x_null=pi/2;                   % kezdeti feltételek
y_null=0;
t_null=0;

% Idõ kiszámolása.
% Minden módszer azonos diszkrét idõpontokat használ, ezért itt külön van
% szedve.
time=zeros(N,1);            % N*1-es mátrix végig 0 elemekkel
time(1)=t_null;             % kezdeti érték megadása
for a=1:(N-1)               % "idõ mátrix" feltöltése
    time(a+1)=time(a)+h;
end
% FELADAT
% Adott a következõ egyenlet: x"+sin(x) = 0
% Átírva 2 elsõfokúra:
% x'= y
% y'=-sin(x) adódik.

% Energiafüggvénye: 0.5*y^2 + 1-cos(x)

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

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



% Explicit Euler módszer
% X = x + h*y
% Y = y -h*sin(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 .* (-sin(EEuler(a,1)));
end

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


%---------------------------------------------------------------------
% Adaptív lépéshossz:
max_lepesszam=10000;
AS_time=nan(max_lepesszam,1);           
AS_time(1)=t_null; 

AS_EEuler=nan(max_lepesszam,2);
AS_EEuler(1,1)=x_null;
AS_EEuler(1,2)=y_null;

%h=lépésköz
tolerancia_also=10^-4;
tolerancia_felso=2*10^-4;
            
a=1;
%h=0.01;
while a<length(AS_time)
    
    %hiba_uj=y_uj(h_regi lépésközzel)-2*y_uj(h_regi/2 lépésközzel)
    %nagy lépésköz:
    AS1x = AS_EEuler(a,1) + h* AS_EEuler(a,2);
    AS1y = AS_EEuler(a,2) + h* (-sin(AS_EEuler(a,1)));
    %fele akkora lépésköz 1-szer:
    AS2x1 = AS_EEuler(a,1) + (h/2) * AS_EEuler(a,2); 
    AS2y1 = AS_EEuler(a,2) + (h/2) * (-sin(AS_EEuler(a,1)));
    %fele akkora lépésköz 2-szer:
    AS2x = AS2x1 + (h/2) * AS2y1;
    AS2y = AS2y1 + (h/2) * (-sin(AS2x1));
    % a két megoldás különbsége a hiba:
    hibax = AS2x-AS1x;
    hibay = AS2y-AS1y;
    hiba_akt=sqrt(hibax^2+hibay^2)
    
    %
    h
    %ha hiba_akt>tolerancia_alsó -> h_uj=h_regi/2
    %ha hiba_akt<tolerancia_felsõ -> h_uj=h_regi*2 
    %különben -> h_uj=h_regi
    if hiba_akt>tolerancia_also
        h=h/2;
        AS_EEuler(a+1,1)=AS2x1;
        AS_EEuler(a+1,2)=AS2y1;
    elseif hiba_akt<tolerancia_felso
        h=h*2;
        AS_EEuler(a+1,1)=AS_EEuler(a,1) + h .* AS_EEuler(a,2);
        AS_EEuler(a+1,2)=AS_EEuler(a,2) + h .* (-sin(AS_EEuler(a,1)));
    else
        AS_EEuler(a+1,1)=AS1x;
        AS_EEuler(a+1,2)=AS1y;
    end
    %}
    
    %{
    %vagy
    %h_új=konstans*h_regi*tolerancia/hiba_akt 
    %kezdeti h legyen 0.01
    konstans=1;
    tolerancia=10^-8;
    h=konstans*h*tolerancia/hiba_akt %nagyon ugrál valamiért???
    AS_EEuler(a+1,1)=AS_EEuler(a,1) + h * AS_EEuler(a,2);
    AS_EEuler(a+1,2)=AS_EEuler(a,2) + h * (-sin(AS_EEuler(a,1)));
    %}
    
    AS_time(a+1)=AS_time(a)+h;
    a=a+1;
end

% Energia
NRG_AS_EE=AS_EEuler(:,2).*AS_EEuler(:,2)./2.+1.-cos(AS_EEuler(:,1));




%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%        Kirajzoltatás        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%



figure;
%t-szerint
subplot(2,2,1);
hold on;
plot(time,EEuler(:,1),'red');
plot(AS_time,AS_EEuler(:,1),'blue');
plot(timeode45, ode45mg(:,1),'magenta','DisplayName', 't szerint');
hold off;
xlabel('t');
ylabel('x');
axis equal
legend('EE','AS EE','ode45');

%x-y szerint
subplot(2,2,2)
hold on;
plot(EEuler(:,1),EEuler(:,2),'red');
plot(AS_EEuler(:,1),AS_EEuler(:,2),'blue');
plot(ode45mg(:,1), ode45mg(:,2), 'magenta','DisplayName', 'x-y szerint');
hold off;
xlabel('x');
ylabel('y');
axis equal
legend('EE','AS EE','ode45');

%Energiak
subplot(2,2,3);
hold on;
plot(time,NRG_EE,'red');
plot(AS_time,NRG_AS_EE,'blue');
plot(timeode45, NRG_ode45,'magenta','DisplayName', 'Energiak');
hold off;
xlabel('t');
ylabel('Energia');
axis equal
legend('EE','AS EE','ode45');

%t-x-y szerint
subplot(2,2,4);
hold on;
plot3(time,EEuler(:,1),EEuler(:,2),'red');
plot3(AS_time,AS_EEuler(:,1),AS_EEuler(:,2),'blue');
plot3(timeode45, ode45mg(:,1), ode45mg(:,2),'magenta','DisplayName', 't-x-y szerint');
hold off;
grid on
xlabel('t');
ylabel('x');
zlabel('y');
legend('EE', 'AS EE', 'ode45');