function [ t_star, x_star, u_star ] = hw6_sample_sol()

a1 = 9.81;
a2 = 0.1;
a3 = 1;

bcfun = @(z0, zf) [z0(1:2,:); zf(1)-pi; zf(2)];

N = 50;
solinit = bvpinit(linspace(0,5,N), zeros(4,1));
sol = bvp4c(@(t, z) pendulum_bvp(t, z, a1, a2, a3), bcfun, solinit);

% Resample the solution (unnecessary)
Ts = 1/10;
t_star = (0:Ts:N*Ts)';
x_star(:,1) = interp1(sol.x, sol.y(1,:)', t_star);
x_star(:,2) = interp1(sol.x, sol.y(2,:)', t_star);
u_star = interp1(sol.x, -1/2*a3*sol.y(4,:)', t_star);

end

function dx = pendulum_bvp(t, x, a1, a2, a3)
% -- On a hoop --
% x(1) - theta
% x(2) - Dtheta
% x(3) - p1
% x(4) - p2

dx = zeros(4,1);

dx(1) = x(2);
dx(2) = -a2*x(2) - a1*sin(x(1)) - 1/2*a3^2*x(4);
dx(3) = a1*x(4)*cos(x(1));
dx(4) = -x(3) + a2*x(4);
end