load_kernels
clc
clear
close all

t0 = cspice_str2et('2025-jan-1 00:00:00'); 
t1 = 9*60*60*24;
t = linspace(t0,t0+t1,600);
kep = [100+1738,0,pi/2,0,0,0];
[state,T] = kep2state(kep,2,t0);

[t_LLO,Y_LLO] = int_eqmoto(state,t0,t0+t1/3,1,4);

for i = 1:length(t)
    y = cspice_spkezr('-60000',t(i),'j2000','none','earth');
    C = cspice_sxform('j2000','MOON_ME',t(i)); 
    s_moon = cspice_spkezr('moon',t(i),'j2000','none','earth');
    Y_moon(i,:) =  (C*(y-s_moon))';
    S_moon(i,:) = s_moon';
end
for j = 1:length(t_LLO)
    s_moon = cspice_spkezr('moon',t_LLO(j),'j2000','none','earth');
    Y_LLO_moon(j,:) = (C*(Y_LLO(j,:)'-s_moon))';
end

r_apo = max(vecnorm(Y_moon(:,1:3),2,2))

%%
figure
plot_earth_moon(1)
grid on
axis equal
hold on
plot_earth_moon(2,s_moon)
plot3(S_moon(:,1),S_moon(:,2),S_moon(:,3))
plot3(Y_LLO(:,1),Y_LLO(:,2),Y_LLO(:,3))

figure
plot3(Y_LLO_moon(:,1),Y_LLO_moon(:,2),Y_LLO_moon(:,3))
hold on
plot3(Y_moon(:,1),Y_moon(:,2),Y_moon(:,3))
grid on
axis equal
hold on
plot_earth_moon(2)
