function double_pendulum()
	m = 1;	
	l = 4;
	g = 9.81;
	
	initial = [
		pi/2;	% phi1
		pi;	% phi2
		0;	% p1
		0	% p2
	];
	
	function e = energy(pos)
		dphi1 = 6 / (m*l^2) * (2*pos(3) - 3*cos(pos(1) - pos(2))*pos(4)) / (16 - 9*cos(pos(1) - pos(2))^2);
		dphi2 = 6 / (m*l^2) * (8*pos(4) - 3*cos(pos(1) - pos(2))*pos(3)) / (16 - 9*cos(pos(1) - pos(2))^2);
		
		e = 1/6 * m * l^2 * (dphi2^2 + 4*dphi1^2 + 3 * dphi1 * dphi2 * cos(pos(1) - pos(2))) - 1/2 * m * g * l * (3*cos(pos(1)) + cos(pos(2)));
	end
	
	function j = jacobian(t, pos)
		% See http://en.wikipedia.org/wiki/Double_pendulum
		dphi1 = 6 / (m*l^2) * (2*pos(3) - 3*cos(pos(1) - pos(2))*pos(4)) / (16 - 9*cos(pos(1) - pos(2))^2);
		dphi2 = 6 / (m*l^2) * (8*pos(4) - 3*cos(pos(1) - pos(2))*pos(3)) / (16 - 9*cos(pos(1) - pos(2))^2);
		j = [
			dphi1;
			dphi2;
			-1/2 * m * l^2 * (dphi1 * dphi2 * sin(pos(1) - pos(2)) + 3 * g / l * sin(pos(1)));
			-1/2 * m * l^2 * (-dphi1 * dphi2 * sin(pos(1) - pos(2)) + g / l * sin(pos(2)))
		];
	end
	
	fig_display = figure(1);
	fig_energy = figure(2);
	xlim([0, 100]);
	function status = plot_data(ts, ys, what)
		if ~isempty(what)
			return
		end
		%for i = 1:length(ts)
		%	t = ts(i);
		%	y = ys(i);
		t = ts; y = ys; if 1 == 1

			x1 = l * sin(y(1));
			y1 = -l * cos(y(1));
			x2 = l * (sin(y(1)) + sin(y(2)));
			y2 = -l * (cos(y(1)) + cos(y(2)));
			
			set(0, 'CurrentFigure', fig_display);
			plot([ 0, x1, x2 ], [ 0, y1, y2 ], '-o', 'MarkerSize', 10, 'LineWidth', 3, 'MarkerFaceColor', 'black');
			xlim([-10 10]);
			ylim([-10 10]);
			drawnow;
			
			set(0, 'CurrentFigure', fig_energy);
			hold on;
			plot([ t ] , [ energy(y) ], 'o', 'MarkerSize', 1);
			hold off;
			drawnow;
			
			pause(0.1);
		end
		status = 0;
	end
	
	ode45(@jacobian, 0:0.001:1000, initial, odeset('OutputFcn', @plot_data));
end

