%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%         Practice 2          %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Nonlinear Dynamical Systems %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%         2019.09.25.         %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Bolzano shooting

% Parameters:
Xinit= -5; % initial conditions
Yinitu = 2.5; % Upper
Yinitl = 2; % Lower
tinit=0; %start time
Max_Time= 400; % end time point
b=0.5; % damping parameter
eps = 1.0e-6; % required accuracy
goal_x=-pi; % target value (get close to this)
goal_y=0;

%make grid, calculate the energy levels
x=-6*pi:0.05:6*pi;
y=-6:0.05:6;
[X,Y] = meshgrid(x,y);
energy_pendulum=@(X,Y) 0.5.*Y.^2-cos(X)+1;
Z = energy_pendulum(X,Y); 

equ_8=@(t, xy) [xy(2); -b*xy(2)-sin(xy(1))]; % damped pendulum equation

done = false; %termination condition of the while loop
while ~done %run till done==false
    
Yinitm = (Yinitu + Yinitl)/2; %midpoint

[t,W] = ode45(equ_8,[tinit Max_Time],[Xinit Yinitm]); %solve the equation

if W(end,1) > goal_x % if the trajectory overshoots at the end -> midpont is the top of the new (half size) interval
  Yinitu=Yinitm;
else
    Yinitl=Yinitm; % if the trajectory undershoots at the end -> midpont is the bottom of the new (half size) interval
end

for i=1:length(W) % iterate throw through the trajectory point (it won't end at the saddle point)
   if abs(W(i,1) - goal_x) < eps && abs(W(i,2)-goal_y) < eps % stop the loop if there is there is a point in the trajectory close enough to the desired position (won't stay there, just gets very close)
        done=true;        
   end
end

end
format long; %long format for printing out more accurate result 
Yinitm %result y value (need to start the pendulum with this initial speed)

% Visualisation
figure;
hold on;
energy_levels=[(linspace(sqrt(0),sqrt(2),5)).^2, (linspace(sqrt(2),sqrt(8),5)).^2, (linspace(sqrt(8),sqrt(16),5)).^2];% energy levels to calculate and plot
contour(X,Y,Z, energy_levels); % draw the energy level curves
[t,W] = ode45(equ_8,[tinit Max_Time],[Xinit Yinitm]);
plot(W(:,1),W(:,2),'r');
xlabel('x');
xticks(-6*pi:pi:6*pi)
ylabel('y')
hold off;