%% funzione di fitness da ottimizzare da NRHO a LEO
function y = fitness_smooth(X,t0,ref,w,H,inc)                                  % Xi = [t1,dt(i),dV(i)] tempi in giorni, dV in km/s

if nargin < 6
     inc = [];
 end
dinc = 0;

% propagazione traiettoria
[Y_t,t_t,Xf_t,~,ie] = propagazione_traj(X,t0);
Xfin = Xf_t(end,:)';                                                       % estraggo stato di arrivo

% calcolo dV impulso circolarizzante
dV_fin = circolarizzazione(Xfin,ref,t_t(end),inc);

% estraggo dV da X
N = length(X)/4+3/4;
dv = X(N+1:end);
dV = reshape(dv,3,[]);                                                     % matrice che ha i dV(i) nelle colonne

r_pl = [6378,1738.4];


r = H(1) + r_pl(ref);                                                            % posizione target
if ref == 2
    s_moon = cspice_spkpos('moon',t_t(end),'j2000','none','earth');
    s_rel = Xfin(1:3)-s_moon;    
    for i = 1:length(t_t)
        Y_rel(i,:) = Y_t(i,:)- cspice_spkezr('moon',t_t(i),'j2000','none','earth')';
    end
else
    s_rel = Xfin(1:3);
    Y_rel = Y_t;
end
dpos = abs(norm(s_rel)-r);                                                 % calcolo errore all'arrivo

if ~isempty(inc)
    XLLO = Xfin + [zeros(3,1); dV_fin];                                         % stato iniziale dell'orbita LLO
    kep_fin = state2kep(XLLO,ref,t_t(end));                                  % estraggo parametri orbitali
    inc_fin = kep_fin(3)*180/pi;                                           % vincolo inclinazione
    dinc = norm(inc-inc_fin);
end

dt = sum(X(2:N));

% calcolo funzione di fitness
normdV = sum(vecnorm(dV,2,1));                                             % contributo dV
y_dv = (norm(dV_fin) + normdV)*w(1);                                       % contributo tot impulsi
y_pos = dpos*w(2);                                                         % contributo quota
y_inc = dinc*w(3);                                                         % contributo inclinazione
y_dt = dt*w(4);                                                            % contributo tempo

% funzione di fitness
y = y_dv + y_pos + y_inc + y_dt;

% penalità collisione
if ~isempty(ie)
    y = 1e10;
end
if ~isscalar(H)
    rrel_apo = max(vecnorm(Y_rel(:,1:3),2,2));
    apo_min = H(2);
    if length(H) == 3
        apo_max = H(3);
        if rrel_apo > apo_max
            y = 1e10;
        end
    end
    if rrel_apo < apo_min
        y = 1e10;
    end
end
