%% funzione che passa da parametri orbitali a stato del satellite in ECI
function [state, tau] = kep2state(kep,parametro,t)                           % (km, rad), parametro = 1 se terra 2 se luna
% se parametro = 2 state restituito in reference terra

% condizione di default
if nargin < 2
    parametro = 1;
    t = 0;
end

% scelta parametro gravitazionale
if parametro == 1
    mu = cspice_bodvrd('399','GM',1);                                      % costante gravitazionale terra
elseif parametro == 2
    mu = cspice_bodvrd('301','GM',1);                                      % costante gravitazionale luna
else
    disp('scegliere parametro tra 1 e 2')
end

% estrazione parametri orbitali
a = kep(1);                                                                % semi-major axis
e = kep(2);                                                                % eccentricity
i = kep(3);                                                                % inclination
raan = kep(4);                                                             % right ascension of ascending node
argp = kep(5);                                                             % argument of periapsis
trueA = kep(6);                                                            % true anomaly


% controllo casi limite
if isnan(raan)
    raan = 0;                                                              % Assign a default value if raan is NaN
end
if isnan(argp)
    argp = 0;                                                              % Assign a default value if argp is NaN
end
if e == 0
    e = 1e-16;
end

% equazione ellisse
p = a * (1 - e^2);                                                         % semi-latus rectum
h = sqrt(p*mu);                                                            % angular momentum
r_modulo = p/(1+e*cos(trueA));                                             % modulo posizione

% stato nel piano perifocale                                            
r_perifocale = [r_modulo * cos(trueA); r_modulo * sin(trueA); 0];          % position in the orbital plane
v_perifocale = [-mu*sin(trueA)/h; mu*(e+cos(trueA))/h; 0];                 % velocity in the orbital plane

% matrici di rotazione 3-1-3 euler per passare da perifocale a ECI
L1 = [cos(raan) -sin(raan) 0; sin(raan) cos(raan) 0; 0 0 1];               % rotazione attorno a z
L2 = [1 0 0; 0 cos(i) -sin(i); 0 sin(i) cos(i)];                           % rotazione attorno a x
L3 = [cos(argp) -sin(argp) 0; sin(argp) cos(argp) 0; 0 0 1];               % rotazione attorno a z
L = L1*L2*L3;                                                              % matrice completa

% output
r = L*r_perifocale;                                                        % position in the inertial frame
v = L*v_perifocale;                                                        % velocity in the inertial frame
state = [r; v];                                                            % stato completo
tau = 2 * pi * sqrt(a^3 / mu);                                             % orbital period

% cambio sistema di riferimento
if parametro == 2
    C_moon_earth = cspice_sxform('MOON_ME','j2000',t);                     % da luna a terra, sistema inerziale 
    s_moon = cspice_spkezr('moon',t,'j2000','none','earth');               % posizione luna
    state = C_moon_earth*state+s_moon;                                     % rotazione frame
end





