% Bolzano shooting

%alap paraméterek:
Xinit= -5; %kezdeti értékek
Yinitu = 2.5; % Upper
Yinitl = 2.0; % Lower

tinit=0; %kezdeti idõ
Max_Time= 400; %eddig megy a szimuláció
b=0.1; %csillapítás
 
eps = 1.0e-6; %elvárt pontosság
goal_x=pi; %mit közelítsen meg
goal_y=0;

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)
energia_inga=@(X,Y) 0.5.*Y.^2-cos(X)+1;
Z = energia_inga(X,Y); %energiát kiszámol, Z-ben tárol

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

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_x %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:length(W) %kiszámított pontokon végigmegy
   if abs(W(i,1) - goal_x) < eps && abs(W(i,2)-goal_y) < 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!
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

[t,W] = ode45(egyenlet_8,[tinit Max_Time],[Xinit Yinitm]);
%plot(W(:,1),W(:,2),'b');

%{
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),'b');    
    pause(0.001); %várakoztatás a lépések közt
end
%}

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')