format compact

tol = 0.000000000001;
a = 0;
b = 1;
sspan = [a b];
alpha = 0.;
beta = 0.;
T = 0.1;

figure(1)
plot(b, beta, 'r*');
hold on
title('Non-linear shooting method');
xlabel('s');
ylabel('theta');
grid on;

k = 0;
opts = odeset('RelTol',1e-10,'AbsTol',1e-10);
while true
    y0 = [alpha T 0 1];

    [s, y] = ode45(@nleq, sspan, y0);

    k = k + 1
    error = y(end, 1) - beta
    T = T - (y(end, 1) - beta)/y(end, 3)
    
    if (abs(error) < tol)
        break;
    end

    plot(s, y(:, 1));
    pause(3);

end
hold off

% rod shape
u0 = [0 T 0 0];
[s, u] = ode45(@rodshape, sspan, u0);

figure(2)
plot(u(:, 3), u(:, 4));
title('Rod shape');
xlabel('x');
ylabel('y');
grid on;

dx = 1 - u(end, 3)
ymax = max(u(:, 4))

function dyds = nleq(~, y)
% NLEQ Evaluate the rhs of the ODE
% theta'' = -p*sin(theta), z'' = -p*cos(theta)*z
% y = [ theta theta' z z']
    p = 4*pi^2;
    dyds(1, 1) = y(2);
    dyds(2, 1) = -p*sin(y(1));
    dyds(3, 1) = y(4); 
    dyds(4, 1) = -p*cos(y(1))*y(3);
end

function duds = rodshape(~, u)
% RODSHAPE Evaluate the rhs of the ODE
% theta'' = -p*sin(theta), x' = cos(theta), y' = sin(theta)
% u = [theta theta' x y]
    p = 4*pi^2;
    duds(1, 1) = u(2);
    duds(2, 1) = -p*sin(u(1));
    duds(3, 1) = cos(u(1)); 
    duds(4, 1) = sin(u(1));
end

