%% funzione che passa da stato del satellite in ECI a parametri orbitali
function kep = state2kep(state,parametro,t)                                % parametro = 1 se terra, 2 se luna
% kep = semiasse maggiore [km], eccentricità, 
% inclinazione, RAAN, arg. periastro, an. vera [angoli in rad]

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

% estrazione stato
if parametro == 1
    mu = cspice_bodvrd('399','GM',1);                                      % costante gravitazionale
    r = state(1:3);                                                        % Position vector [km]
    v = state(4:6);                                                        % Velocity vector [km/s]

elseif parametro == 2
    mu = cspice_bodvrd('301','GM',1);                                      % costante gravitazionale
    r_ECI = state(1:3);                                                    % Position vector ECI [km]
    v_ECI = state(4:6);                                                    % Velocity vector ECI [km/s]
    state_ECI = [r_ECI;v_ECI];                                             % vettore di stato ECI

    % cambio reference
    C_moon_earth = cspice_sxform('j2000','MOON_ME',t);                     % da terra a luna
    s_moon = cspice_spkezr('moon',t,'j2000','none','earth');               % posizione luna
    state = (C_moon_earth*(state_ECI-s_moon))';                            % rotazione frame

    r = state(1:3);                                                        % Position vector LUNA [km]
    v = state(4:6);                                                        % Velocity vector LUNA [km/s]
else
    disp('scegliere parametro tra 1 e 2')
end

% calcolo parametri utili 
h = cross(r, v);                                                           % Specific angular momentum [km^2/s]
eps = -mu/norm(r)+norm(v)^2/2;                                             % specific energy 
n = cross([0;0;1],h);                                                      % vettore dei nodi

a = -mu/(2*eps);                                                           % semi-major axis
e_vet = cross(v,h)/mu-r/norm(r);                                           % vect Eccentricity
e = norm(e_vet); % Eccentricity

cos_i = h(3)/norm(h);
i = acos(cos_i);                                                           % Inclination [rad]


% RAAN
raan = acos(n(1)/norm(n));
if n(2) < 0
    raan = 2*pi-raan;                                                      % correzione
end

% argument of periapsis
argPeri = acos(dot(n, e_vet) / (norm(n) * e));
if e_vet(3) < 0
    argPeri = 2*pi - argPeri;                                              % correzione
end
if e < 1e-10
    argPeri = 0;
end

% true anomaly

if e < 1e-10
    n_vers = n / norm(n);
    cos_f = dot(n_vers, r) / norm(r);
    cos_f = max(-1, min(1, cos_f));  % clip numerico
    trueAnomaly = acos(cos_f);
else
    trueAnomaly = acos(dot(e_vet, r) / (e * norm(r)));
end
if dot(r, v) < 0
    trueAnomaly = 2*pi - trueAnomaly;                                      % correzione
end

% vettore completo
kep = [a, e, i, raan, argPeri, trueAnomaly]';

