% Kepler-Problem, according to
% "Geometric numerical integration" I.2.4, p.29ff
%
function kepler_outer()

% Define planets and variables
m0 = 1.00000597682;
G = 2.95912208286 * 10^-4;

if 0 == 1
planets = {
	% Mass, initial position, initial velocity
	{1, [0,0,0], [0,0,0], 'Sun'},
	{10^-7, [0.38, 0, 0], [0, pi*115/360/180, 0], 'Merkur'}, % Additional data from Wikipedia (Rough estimates)
	{10^-6, [0.72, 0, 0], [0, pi*224/360/180, 0], 'Venus'},  % especially the initial positions are terribly flawed!
	{0.000001, [1, 0, 0], [0, pi/180, 0], 'Earth'},
	{10^-7, [1.52, 0, 0], [0, pi*686/360/180, 0], 'Mars'},
	{0.000954786104043, [-3.5023653, -3.8169847, -1.5507963], [0.00565429, -0.00412490, -0.00190589], 'Jupiter'},
	{0.000285583733151, [9.0755314, -3.0458353, -1.6483708], [0.00168318, 0.00483525, 0.00192462], 'Saturn'},
	{0.0000437273164546, [8.3101420, -16.2901086, -7.2521278], [0.00354178, 0.00137102, 0.00055029], 'Uranus'},
	{0.0000517759138449, [11.4707666, -25.7294829, -10.8169456], [0.00288930, 0.00114527, 0.00039677], 'Neptune'},
	{1/(1.3*10^8), [-15.5387357, -25.2225594, -3.1902382], [0.00276725, -0.00170702, -0.00136504], 'Pluto'},
};
else
planets = {
	% Mass, initial position, initial velocity
	{1.00000597682, [0, 0, 0], [0, 0, 0], 'Sun'},
	{0.000954786104043, [-3.5023653, -3.8169847, -1.5507963], [0.00565429, -0.00412490, -0.00190589], 'Jupiter'},
	{0.000285583733151, [9.0755314, -3.0458353, -1.6483708], [0.00168318, 0.00483525, 0.00192462], 'Saturn'},
	{0.0000437273164546, [8.3101420, -16.2901086, -7.2521278], [0.00354178, 0.00137102, 0.00055029], 'Uranus'},
	{0.0000517759138449, [11.4707666, -25.7294829, -10.8169456], [0.00288930, 0.00114527, 0.00039677], 'Neptune'},
	{1/(1.3*10^8), [-15.5387357, -25.2225594, -3.1902382], [0.00276725, -0.00170702, -0.00136504], 'Pluto'},
};
end

% Derive the ODE system from the planets defined above
p = zeros(length(planets(:)), 3);
q = p;
for i = 1:length(planets(:))
	p(i,:) = planets{i}{1} * planets{i}{3};
	q(i,:) = planets{i}{2};
end

function energy = hamiltonian(p, q)
	energy = 0;
	for i = 1:length(planets(:))
		energy = energy + 0.5 * 1 / planets{i}{1} * norm(p(i,:), 2)^2;
		for j = 1:length(planets(:))
			if i == j
				continue
			end
			energy = energy - G * planets{i}{1} * planets{j}{1} / norm(q(i,:) - q(j,:), 2);
		end
	end
end
	
function ode_sys = jacobian(p, q)
	% The hamiltonian for this system is
	% H(p, q) = ½ Σ 1/m_i p_i² - G Σ Σ m_i m_j / |q_i - q_j|
	ode_sys = zeros(3 * 2 * length(planets(:)), 1);
	
	for i = 1:length(planets(:))
		% Derivative for the i'th particle's impulse coordinates (= -dH/dq_i)
		total_deriv = [ 0, 0, 0 ];
		for j = 1:length(planets(:))
			if i == j
				continue
			end
			diff_vect = q(i,:) - q(j,:);
			deriv = planets{i}{1} * planets{j}{1} / norm(diff_vect, 2)^3 * diff_vect;
			total_deriv = total_deriv + deriv;
		end
		ode_sys(3*(i-1)+1:3*(i-1)+3) = -G * total_deriv;
		% Derivatives for the i'th particle's space corrdinates (= dH/dp_i)
		deriv = p(i,:) / planets{i}{1};
		ode_sys(3*(length(planets(:)) + i - 1) + 1:3*(length(planets(:)) + i - 1) + 3) = deriv; 
	end
end

% A function to plot planet movement
figure('name', 'kepler');

subplot(2, 2, 2);
scatter3([ 0 ], [0], [0], 1);
title('Trajectories');
subplot(2, 2, [3 4]);
title('Energy');
xlabel('Time t');
ylabel('Energy');
xlim([0 100000]);
function status = plot_data(ts, pq)
	for t_cur = 1:length(ts)
		pq_vec = pq(t_cur,:);		
		q = reshape(pq_vec(length(planets(:))*3 + 1:end), 3, length(planets(:)))';
		p = reshape(pq_vec(1:length(planets(:))*3), 3, length(planets(:)))';

		subplot(2, 2, 1);
		scatter3(q(2:end,1), q(2:end,2), q(2:end,3), 'filled');
		hold on
		title(['Current positions, t=', num2str(ts(t_cur))]);
		scatter3(q(1,1), q(1,2), q(1,3), 'ro', 'filled');
		m_size = 50; xlim([-m_size m_size]); ylim([-m_size m_size]); zlim([-m_size m_size]);
		hold off
		drawnow;
		
		subplot(2, 2, 2);
		hold on
		scatter3(q(:, 1), q(:, 2), q(:, 3), 1, 1:length(planets(:)));
		hold off
		drawnow
		
		subplot(2, 2, [ 3 4 ]);
		hold on
		plot([ ts(t_cur) ], [ hamiltonian(p, q) ], '.', 'MarkerSize', 1);
		drawnow
	end
	
	if isempty(findobj('type', 'figure', 'name', 'kepler'))
		status = 1;
		close(findobj('type', 'figure'));
		return;
	end
	
	status = 0;
end

% Some basic integrators matlab does not have per se
function [t, x] = explicit_euler(jacobian, time, initial, params)
	step_size = time(2) - time(1);
	for t = time
		initial = initial + step_size * jacobian(t, initial);
		if mod(t, 100) == 0
			params.OutputFcn(t, initial, '');
		end
	end
end

% Simulate!
function ode_sys = calc_jacobian(~, pq_vec)
	p = reshape(pq_vec(1:length(planets(:))*3), 3, length(planets(:)))';
	q = reshape(pq_vec(length(planets(:))*3 + 1:end), 3, length(planets(:)))';
	ode_sys = jacobian(p, q);
end

pq_vec = [ reshape(p', length(planets(:))*3, 1); reshape(q', length(planets(:))*3, 1) ];

function status = do_plot(t, y, what)
	if length(what) == 0
		status = plot_data([ t(1) ], [ y' ]);
	end
end

% Some ideas:
% ode23t → Trapezodial rule
% ode45 → RK 45
% explicit_euler → See above. (Uses step size defined here!)
ode45(@calc_jacobian, 0:30:1000000, pq_vec, odeset('OutputFcn', @do_plot));

end
