function double_pendulum_my()
	m = 1;	
	l = 4;
	g = 9.81;
	
	initial = [
		pi/2;	% phi1
		pi;	% phi2
		0;	% p1
		0	% p2
	];
	
	function e = energy(pos)
		function g = do_mul(a,b)
			g = a*b;
		end
		function g = do_add(a,b)
			g = a+b;
		end
		function g = do_sub(a,b)
			if ~exist('b', 'var')
				g = -a;
			else
				g = a-b;
			end
		end
		function g = do_div(a,b)
			g = a/b;
		end
		p1 = pos(3);
		p2 = pos(4);
		t1 = pos(1);
		t2 = pos(2);
		e = do_mul(do_mul(do_mul(1/2, m), l^2), do_add(do_add(do_mul(do_mul(2, do_add(do_sub(p1), do_mul(cos(do_add(t1, do_sub(t2))), p2))^2), 1/do_mul(do_mul(m^2, l^4), do_add(do_sub(2), cos(do_add(t1, do_sub(t2)))^2)^2)), do_mul(do_add(do_sub(do_mul(2, p2)), do_mul(cos(do_add(t1, do_sub(t2))), p1))^2, 1/do_add(do_sub(do_mul(do_mul(2, m), l^2)), do_mul(do_mul(cos(do_add(t1, do_sub(t2)))^2, m), l^2))^2)), do_mul(do_mul(do_mul(do_mul(2, do_add(do_sub(p1), do_mul(cos(do_add(t1, do_sub(t2))), p2))), do_add(do_sub(do_mul(2, p2)), do_mul(cos(do_add(t1, do_sub(t2))), p1))), cos(do_add(t1, do_sub(t2)))), 1/do_mul(do_mul(do_mul(m, l^2), do_add(do_sub(2), cos(do_add(t1, do_sub(t2)))^2)), do_add(do_sub(do_mul(do_mul(2, m), l^2)), do_mul(do_mul(cos(do_add(t1, do_sub(t2)))^2, m), l^2))))))
	end
	
	function j = jacobian(t, pos)
		function g = do_mul(a,b)
			g = a*b;
		end
		function g = do_add(a,b)
			g = a+b;
		end
		function g = do_sub(a,b)
			if ~exist('b', 'var')
				g = -a;
			else
				g = a-b;
			end
		end
		function g = do_div(a,b)
			g = a/b;
		end
		p1 = pos(3);
		p2 = pos(4);
		t1 = pos(1);
		t2 = pos(2);
		
		j = [
			do_mul(do_add(do_mul(cos(do_add(t1, do_sub(t2))), p2), do_sub(p1)), 1/do_mul(do_mul(m, l^2), do_add(do_sub(2), cos(do_add(t1, do_sub(t2)))^2)));
			do_mul(do_add(do_mul(p1, cos(do_add(t1, do_sub(t2)))), do_sub(do_mul(2, p2))), 1/do_mul(do_mul(m, l^2), do_add(do_sub(2), cos(do_add(t1, do_sub(t2)))^2)));
			do_sub(do_mul(do_add(do_add(do_add(do_add(do_add(do_add(do_mul(do_mul(do_mul(2, p1), sin(do_add(t1, do_sub(t2)))), p2), do_mul(do_mul(do_mul(p1, sin(do_add(t1, do_sub(t2)))), p2), cos(do_add(t1, do_sub(t2)))^2)), do_sub(do_mul(do_mul(do_mul(2, cos(do_add(t1, do_sub(t2)))), sin(do_add(t1, do_sub(t2)))), p2^2))), do_sub(do_mul(do_mul(cos(do_add(t1, do_sub(t2))), sin(do_add(t1, do_sub(t2)))), p1^2))), do_mul(do_mul(do_mul(do_mul(8, m^2), g), l^3), sin(t1))), do_sub(do_mul(do_mul(do_mul(do_mul(do_mul(8, m^2), g), l^3), sin(t1)), cos(do_add(t1, do_sub(t2)))^2))), do_mul(do_mul(do_mul(do_mul(do_mul(2, m^2), g), l^3), sin(t1)), cos(do_add(t1, do_sub(t2)))^4)), 1/do_mul(do_mul(m, l^2), do_add(do_add(4, do_sub(do_mul(4, cos(do_add(t1, do_sub(t2)))^2))), cos(do_add(t1, do_sub(t2)))^4))));
			do_mul(do_add(do_add(do_add(do_add(do_add(do_add(do_mul(do_mul(do_mul(2, p1), sin(do_add(t1, do_sub(t2)))), p2), do_mul(do_mul(do_mul(p1, sin(do_add(t1, do_sub(t2)))), p2), cos(do_add(t1, do_sub(t2)))^2)), do_sub(do_mul(do_mul(do_mul(2, cos(do_add(t1, do_sub(t2)))), sin(do_add(t1, do_sub(t2)))), p2^2))), do_sub(do_mul(do_mul(cos(do_add(t1, do_sub(t2))), sin(do_add(t1, do_sub(t2)))), p1^2))), do_sub(do_mul(do_mul(do_mul(do_mul(4, m^2), g), l^3), sin(t2)))), do_mul(do_mul(do_mul(do_mul(do_mul(4, m^2), g), l^3), sin(t2)), cos(do_add(t1, do_sub(t2)))^2)), do_sub(do_mul(do_mul(do_mul(do_mul(m^2, g), l^3), sin(t2)), cos(do_add(t1, do_sub(t2)))^4))), 1/do_mul(do_mul(m, l^2), do_add(do_add(4, do_sub(do_mul(4, cos(do_add(t1, do_sub(t2)))^2))), cos(do_add(t1, do_sub(t2)))^4)))
		];
	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.01:1000, initial, odeset('OutputFcn', @plot_data));
end

