clc
clear
close
syms t x1(t) y1(t) x2(t) y2(t) x3(t) y3(t)

G = 6.6743 * 10^-11;

%epsilon = 1e-4

m1 = 10^12;
m2 = 1*10^12;
m3 = 1*10^12;

r1 = [x1(t),y1(t)];
K1 = 1/2 * m1 * (diff(x1(t),t)^2 + diff(y1(t),t)^2);

r2 = [x2(t),y2(t)];
K2 = 1/2 * m2 * (diff(x2(t),t)^2 + diff(y2(t),t)^2);

r3 = [x3(t),y3(t)];
K3 = 1/2 * m3 * (diff(x3(t),t)^2 + diff(y3(t),t)^2);

L1x = diff(diff(K1,diff(x1(t),t)) , t);
L1y = diff(diff(K1,diff(y1(t),t)) , t);

L2x = diff(diff(K2,diff(x2(t),t)) , t);
L2y = diff(diff(K2,diff(y2(t),t)) , t);

L3x = diff(diff(K3,diff(x3(t),t)) , t);
L3y = diff(diff(K3,diff(y3(t),t)) , t);

r12 = r2 - r1;
r13 = r3 - r1;
r23 = r3 - r2;

dlugosc_r12 = sqrt(r12(1)^2 + r12(2)^2);
dlugosc_r13 = sqrt(r13(1)^2 + r13(2)^2);
dlugosc_r23 = sqrt(r23(1)^2 + r23(2)^2);

Q12 = G * m1 * m2 / dlugosc_r12^2 * (r2-r1)/dlugosc_r12;
Q13 = G * m1 * m3 / dlugosc_r13^2 * (r3-r1)/dlugosc_r13;
Q23 = G * m2 * m3 / dlugosc_r23^2 * (r3-r2)/dlugosc_r23;

Q21 = -Q12;
Q32 = -Q23;
Q31 = -Q13;

Q1 = Q12 + Q13;
Q2 = Q21 + Q23;
Q3 = Q31 + Q32;

eqn_1_x = L1x == Q1(1);
eqn_1_y = L1y == Q1(2);

eqn_2_x = L2x == Q2(1);
eqn_2_y = L2y == Q2(2);

eqn_3_x = L3x == Q3(1);
eqn_3_y = L3y == Q3(2);

syms X1 Y1 X2 Y2 X3 Y3

Q1_num = subs(Q1,[x1(t), y1(t), x2(t), y2(t), x3(t), y3(t)],[X1, Y1, X2, Y2, X3, Y3]);
Q2_num = subs(Q2,[x1(t), y1(t), x2(t), y2(t), x3(t), y3(t)],[X1, Y1, X2, Y2, X3, Y3]);
Q3_num = subs(Q3,[x1(t), y1(t), x2(t), y2(t), x3(t), y3(t)],[X1, Y1, X2, Y2, X3, Y3]);

syms vx1 vy1 vx2 vy2 vx3 vy3

state_dot = [
    vx1;
    vy1;
    vx2;
    vy2;
    vx3;
    vy3;
    Q1_num(1)/m1;
    Q1_num(2)/m1;
    Q2_num(1)/m2;
    Q2_num(2)/m2;
    Q3_num(1)/m3;
    Q3_num(2)/m3
    ];

f = matlabFunction(state_dot, 'Vars', {sym('t'), [X1; Y1; X2; Y2; X3; Y3; vx1; vy1; vx2; vy2; vx3; vy3]});

u0 = [-1e5;         %x1
    0;              %y1
    1e5;            %x2   
    0;              %y2
    0;              %x3
    sqrt(3)*1e5;        %y3    
    -11/2 * 1e-3;        %vx1
    11/2*sqrt(3)*1e-3;       %vy1
    -11/2 * 1e-3;        %vx2
    -11/2*sqrt(3)*1e-3;      %vy2
    11e-3;           %vx3
    0];         %vy3

tspan = [0, 1e9];

%options = odeset('RelTol', 1e-15, 'AbsTol', 1e-20);

[t_sol, u_sol] = ode45(f, tspan, u0);

t_anim = linspace(t_sol(1), t_sol(end), 5000);

u_anim = interp1(t_sol, u_sol, t_anim);
%% 


% figure;
tor_1 = plot(u_anim(:,1), u_anim(:,2), 'r', 'LineWidth',1.5); hold on;
tor_2 = plot(u_anim(:,3), u_anim(:,4), 'g', 'LineWidth',1.5);
tor_3 = plot(u_anim(:,5), u_anim(:,6), 'b', 'LineWidth',1.5);
% xlabel('x [m]');
% ylabel('y [m]');
% legend('Ciało 1', 'Ciało 2', 'Ciało 3');
% title('Trajektorie ciał w układzie trzech ciał');
% axis equal
% grid on;

pozycja_1 = plot(u_anim(1,1),u_anim(1,2),'ro','markersize',10,'markerface','r'); hold on
pozycja_2 = plot(u_anim(1,3),u_anim(1,4),'go','markersize',10,'markerface','g');
pozycja_3 = plot(u_anim(1,5),u_anim(1,6),'bo','markersize',10,'markerface','b');
% xlim([-2e5,2e5])
% ylim([-2e5,2e5])
axis equal

for i = 1 : 1 : length(t_sol)

    set(pozycja_1,'XData', u_anim(i,1),'YData', u_anim(i,2));
    set(pozycja_2,'XData', u_anim(i,3),'YData', u_anim(i,4)); 
    set(pozycja_3,'XData', u_anim(i,5),'YData', u_anim(i,6)); 

    set(tor_1,'XData', u_anim(1:i,1),'YData', u_anim(1:i,2));
    set(tor_2,'XData', u_anim(1:i,3),'YData', u_anim(1:i,4));
    set(tor_3,'XData', u_anim(1:i,5),'YData', u_anim(1:i,6));
    drawnow;

    % pause(0.001);
end