%
%alap paraméterek:
 Xinit= -5; %kezdeti értékek, ezektõl függés???
 Yinit=2.5;
 tinit=0; %kezdeti idõ
 Max_Time= 100; %eddig megy a szimuláció
 b=0.1; %csillapítás
 
x=-10:0.05:10;
y=-4:0.05:4;
[X,Y] = meshgrid(x,y); %hálót készít x,y vektorok alapján (az energiák kiszámításához az adott pontokban)

%egyenletek, és energiák
egyenlet_1=@(t, xy) [xy(2); -xy(1)]; % LC kör, alap rugó egyenlet, stabil egyensúlyi pálya
energia_rugo=@(X,Y) 0.5.*Y.^2+0.5.*X.^2;

egyenlet_2=@(t, xy) [xy(2); -b*xy(2)-xy(1)]; %RLC kör, rugó csillapítás, lecseng

egyenlet_3=@(t, xy) [xy(2); -xy(1)+cos(t)]; %RC kör vagy rugó gerjesztéssel, elszáll

wk=1.1; %w=1.5;
egyenlet_4=@(t, xy) [xy(2); -xy(1)+cos(wk*t)]; %más körfrekvenciás külsõ gerjesztés, összetett periódus

wb=100;
egyenlet_5=@(t, xy) [xy(2); -wb*xy(1)+cos(t)]; % rugó, módosított belsõ rezgés

egyenlet_6=@(t, xy) [xy(2); -b*xy(2)-xy(1)+cos(t)]; % RLC kör vagy csillapított rugó gerjesztéssel, stabil priodikus pályára áll be tranziens után

egyenlet_7=@(t, xy) [xy(2); -sin(xy(1))]; %inga (hajóhinta) alap egyenlet
energia_inga=@(X,Y) 0.5.*Y.^2-cos(X)+1;

egyenlet_8=@(t, xy) [xy(2); -b*xy(2)-sin(xy(1))]; % inga csillapítással, lecseng

%2. pont, ha kell
 Xinit2= -0.2007;
 Yinit2= 0.3;
egyenlet_9=@(t, xy) [xy(2); -sin(xy(1))-b*xy(2)+cos(t)]; %inga gerjesztéssel és csillapítással, káosz, érzékeny függés a kezdeti értékektõl 


Z = energia_inga(X,Y); %energiát kiszámol, Z-ben tárol

%
%options=odeset('RelTol',1e-8 ,'AbsTol',1e-10); %toleranciák beállítása a kissebb hibákért; relTol: lépésenkénti hiba ennél kissebb legyen
%[t, xy]=ode45(egyenlet_7, [tinit, Max_Time], [Xinit; Yinit], options);
[t, xy]=ode45(egyenlet_9, [tinit, Max_Time], [Xinit; Yinit]); %diffegyenletet kiszámol
%[t, xy]=ode45(@(t, xy) rugo_ode(t, xy), [tinit, Max_Time], [Xinit, Yinit]); %diffegyenletet kiszámol,
%[t2, xy2]=ode45(egyenlet_7, [tinit, Max_Time], [Xinit2; Yinit2]);

%
figure;
subplot(2,2,1); %ábrák elhelyezése egy mezõn belül, számolás balról jobbra, fentrõl le
hold on; %több ábra egymásra
energiaszintek=[0, 0.1 0.25, 0.5, 1, 1.4, 2, 2.4, 3, 4]; %ezeken a szinteken számolom ki az energiát
%contour(X,Y,Z, energiaszintek); %energia szintvonalak ábrázolása
plot(xy(:,1), xy(:,2), 'b'); %x-y ábrázolás
%plot(xy2(:,1), xy2(:,2), 'r'); %a 2. ponthoz
xlabel('x');
ylabel('y');
hold off; %ez után már felülír ábrát

subplot(2,2,3);
%hold on; %a 2. ponthoz
plot(xy(:,1), t, 'b'); %x-t ábrázolás
%plot(xy2(:,1), t2, 'r'); %a 2. ponthoz
xlabel('x');
ylabel('t');
%hold off; %a 2. ponthoz

subplot(2,2,2);
%hold on;
plot(t, xy(:,2), 'b'); %t-y ábrázolás
%plot(t2, xy2(:,2), 'r');
xlabel('t');
ylabel('y');
%hold off;

subplot(2,2,4);
%hold on;
plot3(xy(:,1), xy(:,2), t, 'b'); %x-y-t 3D-s ábrázolás
%plot3(xy2(:,1), xy2(:,2), t2, 'r');
xlabel('x');
ylabel('y');
zlabel('t');
%hold off,

%{
%%%%%%%egyensúlyi pontok szinuszos esetben
P = zeros(7,2);
P(:,1)=linspace(-3*pi,3*pi,7);
plot(P(:,1),P(:,2),'.','Color','black');
%}
%}

%{
% Bolzano shooting
eps = 1.0e-6; %elvárt pontosság
goal=pi; %mit közelítsen meg
Yinitu = 2.5; % Upper
Yinitl = 2.0; % Lower

done = false; %meddig megy a while ciklus

while ~done %megy míg a done false
Yinitm = (Yinitu + Yinitl)/2; %intervallum felezõ pont

[t,W] = ode45(egyenlet_8,[tinit Max_Time],[Xinit Yinitm]);

if W(end,1) > goal %ha végül a célon túllõ a felezõpont az új intervallum teteje
  Yinitu=Yinitm;
else
    Yinitl=Yinitm; %ha végül a cél alá fut trajektória a felezõpont az új intervallum alja
end

for i=1:size(W) %kiszámított pontokon végigmegy
   if abs(W(i,1) - goal) < eps && abs(W(i,2)) < eps %akkor áll le a ciklus ha valamikor a cél pont eps sugarú környezetébe ér (nem marad ott, csak nagyon megközelíti)
        done=true;        
   end
end
end

format long; %pontosabb kiiratás
Yinitm
%
% Rajzoljuk is ki!
[t,W] = ode45(egyenlet_8,[tinit Max_Time],[Xinit Yinitm]);
figure;
hold on;
energiaszintek=[0, 0.1 0.25, 0.5, 1, 1.4, 2, 2.4, 3, 4];
contour(X,Y,Z, energiaszintek); %energia szintvonalak
%{
for i=1:length(W) %lépésenként rajzolom ki 1-tõl i-ig,
plot(W(1:i,1),W(1:i,2),'r');    
pause(0.001); %várakoztatás a lépések közt
end
%}
plot(W(:,1),W(:,2),'red');
hold off;
%}

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% EZ BE FOG DOGLENI! 100 idore! MEG 1 eps re is! Az ok a linearizÃ¡lt egyenletben
% levo exponencialis novekedes a nyeregpont kornyeken.

% % Bolzano shooting "Naiv matematikus" verzio
% b = 0.1;
% eps = 0.01;
% Max_Time = 12;
% 
% Xinit = -10;
% 
% Yinitu = 2.5; % Upper
% Yinitl = 2.0; % Lower
% 
% done = false;
% 
% while ~done
% Yinitm = (Yinitu + Yinitl)/2;
%     
% [t,W] = ode45(@(t,y) damped_pendulum_equat(t,y,b),[0 Max_Time],[Xinit Yinitm]);
% 
% if W(end,1) > pi
%    Yinitu=Yinitm;
% else
%     Yinitl=Yinitm;
% end
% 
% % Megallasi feltetel naivan megvalasztva
% if abs(W(end,1) - pi) < eps
%     done=true;        
% end
% 
% end
% 
% disp(strcat('Nyertes Yinit:',num2str(Yinitm)));
% 
% % Rajzoljuk is ki!
% [t,W] = ode45(@(t,y) damped_pendulum_equat(t,y,b),[0 Max_Time],[Xinit Yinitm]);
% plot(W(:,1),W(:,2),'--black')